diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 0e6d1b948..61c86dd83 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -1,4 +1,4 @@ - + @@ -178,6 +178,28 @@ + + + + + + + + + + + + + + + + + + + + + + @@ -190,6 +212,9 @@ + + + diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index 885766832..bda430cbc 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -1,4 +1,4 @@ - + @@ -488,5 +488,14 @@ Source Files + + Source Files + + + Source Files + + + Source Files + \ No newline at end of file diff --git a/AirLib/AirLib_VS2019.vcxproj b/AirLib/AirLib_VS2019.vcxproj index 2ffca041a..a0949b914 100644 --- a/AirLib/AirLib_VS2019.vcxproj +++ b/AirLib/AirLib_VS2019.vcxproj @@ -1,4 +1,4 @@ - + @@ -178,6 +178,28 @@ + + + + + + + + + + + + + + + + + + + + + + @@ -190,6 +212,9 @@ + + + diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index bea182f69..77d10b259 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -53,6 +53,14 @@ namespace airlib virtual Pose getPose() const = 0; virtual void setPose(const Pose& pose, bool ignore_collision) = 0; + virtual void setPoseCustom(const Pose& pose, const std::vector& tilt_angles, bool ignore_collision, bool spin_props) + { + unused(pose); + unused(tilt_angles); + unused(ignore_collision); + unused(spin_props); + throw std::runtime_error("setPoseCustom is not supported on this vehicle"); + } virtual const Kinematics::State* getGroundTruthKinematics() const = 0; virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) = 0; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index e5442ff22..c272ceced 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -35,6 +35,8 @@ namespace airlib static constexpr char const* kVehicleTypePhysXCar = "physxcar"; static constexpr char const* kVehicleTypeArduRover = "ardurover"; static constexpr char const* kVehicleTypeComputerVision = "computervision"; + static constexpr char const* kVehicleTypeRovSimple = "rovsimple"; + static constexpr char const* kVehicleTypePX4Rov = "px4rov"; static constexpr char const* kVehicleInertialFrame = "VehicleInertialFrame"; static constexpr char const* kSensorLocalFrame = "SensorLocalFrame"; @@ -42,6 +44,7 @@ namespace airlib static constexpr char const* kSimModeTypeMultirotor = "Multirotor"; static constexpr char const* kSimModeTypeCar = "Car"; static constexpr char const* kSimModeTypeComputerVision = "ComputerVision"; + static constexpr char const* kSimModeTypeRov = "Rov"; struct SubwindowSetting { @@ -598,7 +601,7 @@ namespace airlib physics_engine_name = settings_json.getString("PhysicsEngineName", ""); if (physics_engine_name == "") { - if (simmode_name == kSimModeTypeMultirotor) + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeRov) physics_engine_name = "FastPhysicsEngine"; else physics_engine_name = "PhysX"; //this value is only informational for now @@ -811,7 +814,7 @@ namespace airlib auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); std::unique_ptr vehicle_setting; - if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) + if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover || vehicle_type == kVehicleTypePX4Rov) vehicle_setting = createMavLinkVehicleSetting(settings_json); //for everything else we don't need derived class yet else { @@ -883,6 +886,12 @@ namespace airlib cv_setting->sensors = sensor_defaults; vehicles[cv_setting->vehicle_name] = std::move(cv_setting); } + else if (simmode_name == kSimModeTypeRov) { + // create default rov vehicle + auto rov_setting = std::unique_ptr(new VehicleSetting("RovSimple", kVehicleTypeRovSimple)); + rov_setting->sensors = sensor_defaults; + vehicles[rov_setting->vehicle_name] = std::move(rov_setting); + } else { throw std::invalid_argument(Utils::stringf( "Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()) @@ -1251,16 +1260,16 @@ namespace airlib //default value clock_type = "ScalableClock"; - //override if multirotor simmode with simple_flight - if (simmode_name == kSimModeTypeMultirotor) { + //override if multirotor or rov simmode with simple_flight or rov_simple + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeRov) { //TODO: this won't work if simple_flight and PX4 is combined together! - //for multirotors we select steppable fixed interval clock unless we have + //for multirotors and rovs we select steppable fixed interval clock unless we have //PX4 enabled vehicle clock_type = "SteppableClock"; for (auto const& vehicle : vehicles) { if (vehicle.second->auto_create && - vehicle.second->vehicle_type == kVehicleTypePX4) { + (vehicle.second->vehicle_type == kVehicleTypePX4 || vehicle.second->vehicle_type == kVehicleTypePX4Rov)) { clock_type = "ScalableClock"; break; } @@ -1359,7 +1368,7 @@ namespace airlib static void createDefaultSensorSettings(const std::string& simmode_name, std::map>& sensors) { - if (simmode_name == kSimModeTypeMultirotor) { + if (simmode_name == kSimModeTypeMultirotor || simmode_name == kSimModeTypeRov) { sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 971728e08..6c874544c 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -170,6 +170,12 @@ namespace airlib }; public: + template + static int sgn(T val) + { + return (val > T(0)) - (val < T(0)); + } + static float magnitude(const Vector2f& v) { return v.norm(); diff --git a/AirLib/include/vehicles/rov/AeroParams.hpp b/AirLib/include/vehicles/rov/AeroParams.hpp new file mode 100644 index 000000000..dfbf8b6fe --- /dev/null +++ b/AirLib/include/vehicles/rov/AeroParams.hpp @@ -0,0 +1,109 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_AeroParams_hpp +#define msr_airlib_AeroParams_hpp + +#include "common/Common.hpp" + +namespace msr +{ +namespace airlib +{ + + //contains air params necessary for calculating aerodynamic forces and moments + struct AirState + { + real_T Va; + real_T alpha; + real_T beta; + real_T rho; + + void zero() + { + Va = 0.f; + alpha = 0.f; + beta = 0.f; + rho = 0.f; + } + + void setAirspeedState(Vector3r airspeed) + { + Va = airspeed.norm(); + + if (airspeed(0) == 0.f) { + alpha = VectorMath::sgn(airspeed(2)) * M_PI / 2.0; + } + else { + alpha = std::atan2(airspeed(2), airspeed(0)); + } + if (Va == 0.f) { + beta = VectorMath::sgn(airspeed(1)) * M_PI / 2.0; + } + else { + beta = std::asin(airspeed(1) / Va); + } + } + + void setAirDensity(real_T air_density) + { + rho = air_density; + } + }; + + struct AeroParams + { + //flap parameters + real_T flap_rise_time = 0.05f; + real_T flap_max_angle = 0.0f; + + //wing parameters + real_T S = 0.0; + real_T b = 0.0; + real_T c = 0.0; + real_T aspect_ratio = 0.0; + real_T epsilon = 0.0; + real_T alpha0 = 0.0; + real_T M = 0.0; + real_T e = 0.0; + + struct AeroCoefficient + { + AeroCoefficient(real_T O_in, real_T alpha_in, real_T beta_in, real_T p_in, real_T q_in, real_T r_in, + real_T delta_a_in, real_T delta_e_in, real_T delta_r_in) + : O{ O_in }, alpha{ alpha_in }, beta{ beta_in }, p{ p_in }, q{ q_in }, r{ r_in }, delta_a{ delta_a_in }, delta_e{ delta_e_in }, delta_r{ delta_r_in } + { + } + + real_T O; + real_T alpha; + real_T beta; + real_T p; + real_T q; + real_T r; + real_T delta_a; + real_T delta_e; + real_T delta_r; + }; + + // //aerodynamic coefficients + AeroCoefficient CL{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + AeroCoefficient CD{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + AeroCoefficient CY{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + AeroCoefficient Cl{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + AeroCoefficient Cm{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + AeroCoefficient Cn{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + //control mixer + //This is for mixing of control flap inputs for fixedwing aircraft that don't + //have a standard flap congiguration (elevator, aileron, rudder) such as aircraft + //with elevons or ruddervators. Default is identity, but must be set for other + //types of aircraft. + //Additional note: this can also be used for flipping the sign of control flap inputs + Matrix3x3r aero_control_mixer = Matrix3x3r::Identity(); + }; + +} +} //namespace + +#endif diff --git a/AirLib/include/vehicles/rov/AeroVertex.hpp b/AirLib/include/vehicles/rov/AeroVertex.hpp new file mode 100644 index 000000000..462ea0b6c --- /dev/null +++ b/AirLib/include/vehicles/rov/AeroVertex.hpp @@ -0,0 +1,222 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef airsimcore_aero_vertex_hpp +#define airsimcore_aero_vertex_hpp + +#include "common/Common.hpp" +#include "common/FirstOrderFilter.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" +#include "physics/PhysicsBodyVertex.hpp" +#include "vehicles/rov/AeroParams.hpp" + +namespace msr +{ +namespace airlib +{ + + class AeroVertex : public PhysicsBodyVertex + { + public: + struct Output + { + real_T alpha; + real_T beta; + real_T Va; + real_T flap_input_1; + real_T flap_input_2; + real_T flap_input_3; + real_T flap_angle_1; + real_T flap_angle_2; + real_T flap_angle_3; + }; + + public: + AeroVertex() + { + //allow default constructor with later call for initialize + } + + AeroVertex(const AeroParams& params, const Environment* environment, const Kinematics* kinematics, const AirState air_state) + { + initialize(params, environment, kinematics, air_state); + } + + void initialize(const AeroParams& params, const Environment* environment, const Kinematics* kinematics, const AirState air_state) + { + params_ = params; + environment_ = environment; + kinematics_ = kinematics; + air_state_ = air_state; + + //computed wrench is about center of mass, normal direction not used + PhysicsBodyVertex::initialize(Vector3r::Zero(), Vector3r::Zero()); + + for (uint i = 0; i < 3; ++i) { + control_flap_filters_.emplace_back(params_.flap_rise_time, 0.0, 0.0); + } + + setOutput(); + } + + //ElevatorAileronRudder: {elevator, aileron, rudder} + //AileronRudderVator: {aileron, right ruddervator, left ruddervator} + //ElevonRudder: {right elevon, left elevon, rudder} + //values from -1 to 1 + void setFlapInputs(const vector& inputs) + { + for (uint i = 0; i < inputs.size(); ++i) { + control_flap_filters_[i].setInput(params_.flap_max_angle * Utils::clip(inputs[i], -1.f, 1.f)); + } + } + + void setAirspeedVertex(const Vector3r airspeed_body_vector) + { + air_state_.setAirspeedState(airspeed_body_vector); + } + + Output getOutput() const + { + return output_; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override + { + PhysicsBodyVertex::resetImplementation(); + + air_state_.zero(); + + //update environmental factors before we call base + updateEnvironmentalFactors(); + + for (uint i = 0; i < control_flap_filters_.size(); ++i) { + control_flap_filters_[i].reset(); + } + } + + virtual void update() override + { + //update environmental factors before we call base + updateEnvironmentalFactors(); + + //this will in turn call setWrench + PhysicsBodyVertex::update(); + + //set the output + setOutput(); + + //update filters - this should be after so that first output is same as initial + for (uint i = 0; i < control_flap_filters_.size(); ++i) { + control_flap_filters_[i].update(); + } + } + + virtual void reportState(StateReporter& reporter) override + { + reporter.writeValue("Va", output_.Va); + reporter.writeValue("alpha", output_.alpha); + reporter.writeValue("beta", output_.beta); + reporter.writeValue("flap1", output_.flap_angle_1); + reporter.writeValue("flap2", output_.flap_angle_2); + reporter.writeValue("flap3", output_.flap_angle_3); + } + //*** End: UpdatableState implementation ***// + + protected: + virtual void setWrench(Wrench& wrench) override + { + calculateAerodynamicForces(wrench.force, wrench.torque); + } + + private: + void calculateAerodynamicForces(Vector3r& output_force, Vector3r& output_torque) + { + output_force.setZero(); + output_torque.setZero(); + + Vector3r flap_angles; + flap_angles << output_.flap_angle_1, output_.flap_angle_2, output_.flap_angle_3; + + //convert to standard {elevator, aileron, rudder} + Vector3r flap_angles_standard = params_.aero_control_mixer * flap_angles; + real_T elevator = flap_angles_standard(0); + real_T aileron = flap_angles_standard(1); + real_T rudder = flap_angles_standard(2); + + Vector3r angular_vel = kinematics_->getState().twist.angular; + real_T p = angular_vel(0); + real_T q = angular_vel(1); + real_T r = angular_vel(2); + + real_T alpha = air_state_.alpha; + real_T beta = air_state_.beta; + + real_T ca = cos(alpha); + real_T sa = sin(alpha); + + real_T qbar = 0.5f * air_state_.rho * pow(air_state_.Va, 2.f); + + real_T p_nondim; + real_T q_nondim; + real_T r_nondim; + if (air_state_.Va > 0.5f) { + p_nondim = p * params_.b / (2.0 * air_state_.Va); + q_nondim = q * params_.c / (2.0 * air_state_.Va); + r_nondim = r * params_.b / (2.0 * air_state_.Va); + } + else { + p_nondim = 0.0; + q_nondim = 0.0; + r_nondim = 0.0; + } + + double tmp1 = std::exp(-static_cast(params_.M * (alpha - params_.alpha0))); //these numbers are often too large/small to handle as floats + double tmp2 = std::exp(static_cast(params_.M * (alpha + params_.alpha0))); + real_T sigma_a = static_cast((1.0 + tmp1 + tmp2) / ((1.0 + tmp1) * (1.0 + tmp2))); + real_T CL_a = (1.f - sigma_a) * (params_.CL.O + params_.CL.alpha * alpha) + sigma_a * (2.f * VectorMath::sgn(alpha) * sa * sa * ca); + real_T CD_a = (1.f - sigma_a) * (params_.CD.p + ((pow((params_.CL.O + params_.CL.alpha * alpha), 2.0)) / (M_PIf * params_.e * params_.aspect_ratio))) + sigma_a * (2.f * VectorMath::sgn(alpha) * sa); + + real_T f_lift = qbar * params_.S * (CL_a + params_.CL.q * q_nondim + params_.CL.delta_e * elevator); + real_T f_drag = qbar * params_.S * (CD_a + params_.CD.q * q_nondim + params_.CD.delta_e * elevator); + + output_force(0) = 0.0; + output_force(1) = 0.0; + output_force(2) = 0.0; + output_torque(0) = 0.0; + output_torque(1) = 0.0; + output_torque(2) = 0.0; + } + + void setOutput() + { + output_.alpha = air_state_.alpha; + output_.beta = air_state_.beta; + output_.Va = air_state_.Va; + output_.flap_input_1 = control_flap_filters_[0].getInput(); + output_.flap_input_2 = control_flap_filters_[1].getInput(); + output_.flap_input_3 = control_flap_filters_[2].getInput(); + output_.flap_angle_1 = control_flap_filters_[0].getOutput(); + output_.flap_angle_2 = control_flap_filters_[1].getOutput(); + output_.flap_angle_3 = control_flap_filters_[2].getOutput(); + } + + void updateEnvironmentalFactors() + { + //update air density + air_state_.setAirDensity(environment_->getState().air_density); + } + + private: + AeroParams params_; + vector> control_flap_filters_; + const Environment* environment_ = nullptr; + const Kinematics* kinematics_ = nullptr; //need kinematics for calculating aerodynamic forces and moments + Output output_; + AirState air_state_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RotorActuator.hpp b/AirLib/include/vehicles/rov/RotorActuator.hpp new file mode 100644 index 000000000..6fe846f6c --- /dev/null +++ b/AirLib/include/vehicles/rov/RotorActuator.hpp @@ -0,0 +1,149 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef rotor_actuator_hpp +#define rotor_actuator_hpp + +#include +#include "common/Common.hpp" +#include "physics/Environment.hpp" +#include "common/FirstOrderFilter.hpp" +#include "physics/PhysicsBodyVertex.hpp" +#include "RotorParams.hpp" + +namespace msr +{ +namespace airlib +{ + + //Rotor gets control signal as input (PWM or voltage represented from 0 to 1) which causes + //change in rotation speed and turning direction and ultimately produces force and thrust as + //output + class RotorActuator : public PhysicsBodyVertex + { + public: //types + struct Output + { + real_T thrust; + real_T torque_scaler; + real_T speed; + RotorTurningDirection turning_direction; + real_T control_signal_filtered; + real_T control_signal_input; + }; + + public: //methods + RotorActuator() + { + //allow default constructor with later call for initialize + } + RotorActuator(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction, + const RotorParams& params, const Environment* environment, uint id = -1) + { + initialize(position, normal, turning_direction, params, environment, id); + } + void initialize(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction, + const RotorParams& params, const Environment* environment, uint id = -1) + { + id_ = id; + params_ = params; + turning_direction_ = turning_direction; + environment_ = environment; + air_density_sea_level_ = EarthUtils::getAirDensity(0.0f); + + control_signal_filter_.initialize(params_.control_signal_filter_tc, 0, 0); + + PhysicsBodyVertex::initialize(position, normal); //call base initializer + } + + //0 to 1 - will be scaled to 0 to max_speed + void setControlSignal(real_T control_signal) + { + control_signal_filter_.setInput(Utils::clip(control_signal, -1.0f, 1.0f)); + } + + Output getOutput() const + { + return output_; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override + { + PhysicsBodyVertex::resetImplementation(); + + //update environmental factors before we call base + updateEnvironmentalFactors(); + + control_signal_filter_.reset(); + + setOutput(output_, params_, control_signal_filter_, turning_direction_); + } + + virtual void update() override + { + //update environmental factors before we call base + updateEnvironmentalFactors(); + + //this will in turn call setWrench + PhysicsBodyVertex::update(); + + //update our state + setOutput(output_, params_, control_signal_filter_, turning_direction_); + + //update filter - this should be after so that first output is same as initial + control_signal_filter_.update(); + } + + virtual void reportState(StateReporter& reporter) override + { + reporter.writeValue("Dir", static_cast(turning_direction_)); + reporter.writeValue("Ctrl-in", output_.control_signal_input); + reporter.writeValue("Ctrl-fl", output_.control_signal_filtered); + reporter.writeValue("speed", output_.speed); + reporter.writeValue("thrust", output_.thrust); + reporter.writeValue("torque", output_.torque_scaler); + } + //*** End: UpdatableState implementation ***// + + protected: + virtual void setWrench(Wrench& wrench) override + { + Vector3r normal = getNormal(); + //forces and torques are proportional to air density: http://physics.stackexchange.com/a/32013/14061 + wrench.force = normal * output_.thrust * air_density_ratio_; + wrench.torque = normal * output_.torque_scaler * air_density_ratio_; //TODO: try using filtered control here + } + + private: //methods + static void setOutput(Output& output, const RotorParams& params, const FirstOrderFilter& control_signal_filter, RotorTurningDirection turning_direction) + { + output.control_signal_input = control_signal_filter.getInput(); + output.control_signal_filtered = control_signal_filter.getOutput(); + //see relationship of rotation speed with thrust: http://physics.stackexchange.com/a/32013/14061 + //output.speed = sqrt(output.control_signal_filtered * params.max_speed_square); + output.speed = output.control_signal_filtered * params.max_speed_square; + output.thrust = output.control_signal_filtered * params.max_thrust; + //output.torque_scaler = output.control_signal_filtered * params.max_torque * static_cast(turning_direction); + output.torque_scaler = output.control_signal_filtered * 0.0 * static_cast(turning_direction); + output.turning_direction = turning_direction; + } + + void updateEnvironmentalFactors() + { + //update air density ration - this will affect generated force and torques by rotors + air_density_ratio_ = environment_->getState().air_density / air_density_sea_level_; + } + + private: //fields + uint id_; //only used for debug messages + RotorTurningDirection turning_direction_; + RotorParams params_; + FirstOrderFilter control_signal_filter_; + const Environment* environment_ = nullptr; + real_T air_density_sea_level_, air_density_ratio_; + Output output_; + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RotorTiltable.hpp b/AirLib/include/vehicles/rov/RotorTiltable.hpp new file mode 100644 index 000000000..0628bbf73 --- /dev/null +++ b/AirLib/include/vehicles/rov/RotorTiltable.hpp @@ -0,0 +1,269 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef airsimcore_RotorTiltable_hpp +#define airsimcore_RotorTiltable_hpp + +#include +#include "common/Common.hpp" +#include "physics/Environment.hpp" +#include "common/FirstOrderFilter.hpp" +#include "physics/PhysicsBodyVertex.hpp" +#include "vehicles/multirotor/RotorActuator.hpp" +#include "RotorTiltableParams.hpp" + +namespace msr +{ +namespace airlib +{ + + //RotorTiltable inherits from RotorActuator. It is a rotor that is allowed to rotate + //about a single axis with specified range. + + class RotorTiltable : public RotorActuator + { + public: //types + struct TiltOutput + { + Output rotor_output; + real_T angle_signal_filtered; + real_T angle_signal_input; + real_T angle; + real_T angle_from_vertical; + bool is_fixed; + }; + + public: //methods + RotorTiltable() + { + //allow default constructor with later call for initialize + } + RotorTiltable(const Vector3r& position, const Vector3r& normal_nominal, + RotorTurningDirection turning_direction, bool is_fixed, const Vector3r& rotation_axis, + const real_T max_angle, const RotorTiltableParams& params, const Environment* environment, uint id = -1) + { + initialize(position, normal_nominal, turning_direction, is_fixed, rotation_axis, max_angle, params, environment, id); + } + + void initialize(const Vector3r& position, const Vector3r& normal_nominal, + RotorTurningDirection turning_direction, bool is_fixed, const Vector3r& rotation_axis, + const real_T max_angle, const RotorTiltableParams& params, const Environment* environment, uint id = -1) + { + normal_nominal_ = normal_nominal; + normal_nominal_.normalize(); + is_fixed_ = is_fixed; //don't allow rotor to rotate if is_fixed_ is true + rotation_axis_ = rotation_axis; //this gets ignored if is_fixed_ is true + rotation_axis_.normalize(); + max_angle_ = max_angle; + tilt_params_ = params; + environment_ = environment; + + angle_signal_filter_.initialize(params.angle_signal_filter_tc, 0, 0); + angle_filter_.initialize(params.angle_filter_tc, 0, 0); + airspeed_ = 0.0f; + + RotorActuator::initialize(position, normal_nominal, turning_direction, params.rotor_params, environment, id); + initializeTiltOutput(is_fixed); + } + + void initializeTiltOutput(const bool is_fixed) + { + tilt_output_.angle_signal_filtered = 0; + tilt_output_.angle_signal_input = 0; + tilt_output_.angle = 0; + tilt_output_.angle_from_vertical = VectorMath::sgn(normal_nominal_(0)) * std::acos(-normal_nominal_(2)); + tilt_output_.is_fixed = is_fixed; + } + + //-1 to 1, will be scaled to -max_angle_, +max_angle_ + void setAngleSignal(real_T angle_signal) + { + if (!is_fixed_) { + angle_signal_filter_.setInput(Utils::clip(angle_signal, -1.0f, 1.0f)); + } + } + + void setAirspeedRotor(const Vector3r& airspeed_body_vector) + { + airspeed_ = normal_current_.dot(airspeed_body_vector); + } + + //allows manually setting tilt from client + void overwriteTilt(const real_T angle, bool spin_props) + { + if (!is_fixed_) { + tilt_output_.angle_from_vertical = angle; + } + // speed isn't being controlled, so set to arbitrary constant value + if (spin_props) { + tilt_output_.rotor_output.speed = tilt_params_.rotor_params.max_speed * 0.7; + } + + // zero these out since they won't be used to avoid confusion when viewing output + tilt_output_.angle = 0.0f; + tilt_output_.angle_signal_input = 0.0f; + tilt_output_.angle_signal_filtered = 0.0f; + tilt_output_.rotor_output.thrust = 0.0f; + tilt_output_.rotor_output.torque_scaler = 0.0f; + tilt_output_.rotor_output.control_signal_input = 0.0f; + tilt_output_.rotor_output.control_signal_filtered = 0.0f; + + // compute angle from nominal given angle from vertical + // probably not worth the extra computation + // real_T angle_nom_from_vert = VectorMath::sgn(normal_nominal_(0)) * std::acos(-normal_nominal_(2)); + // AngleAxisr rotate_cv = AngleAxisr(angle, rotation_axis_); + // AngleAxisr rotate_nc = AngleAxisr(-angle_nom_from_vert, rotation_axis_); + // Vector3r normal = rotate_cv * rotate_nc * normal_nominal_; + // tilt_output_.angle = std::acos( normal.dot(normal_nominal_) / ( normal.norm() * normal_nominal_.norm() ) ); + } + + TiltOutput getOutput() const + { + return tilt_output_; + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override + { + RotorActuator::resetImplementation(); + + angle_signal_filter_.reset(); + angle_filter_.reset(); + normal_current_ = normal_nominal_; + setNormal(normal_nominal_); + airspeed_ = 0.0f; + + setTiltOutput(); + } + + virtual void update() override + { + //call rotor update + RotorActuator::update(); + + //update tilt output + setTiltOutput(); + + //update angle filters + if (!is_fixed_) { + angle_signal_filter_.update(); + angle_filter_.setInput(max_angle_ * angle_signal_filter_.getOutput()); + angle_filter_.update(); + + //calculate new normal and set normal in PhysicsBodyVertex + AngleAxisr rotate = AngleAxisr(angle_filter_.getOutput(), rotation_axis_); + normal_current_ = rotate * normal_nominal_; + setNormal(normal_current_); + } + } + + virtual void reportState(StateReporter& reporter) override + { + reporter.writeValue("Dir", static_cast(tilt_output_.rotor_output.turning_direction)); + reporter.writeValue("Ctrl-in", tilt_output_.rotor_output.control_signal_input); + reporter.writeValue("Ctrl-fl", tilt_output_.rotor_output.control_signal_filtered); + reporter.writeValue("speed", tilt_output_.rotor_output.speed); + reporter.writeValue("thrust", tilt_output_.rotor_output.thrust); + reporter.writeValue("torque_scaler", tilt_output_.rotor_output.torque_scaler); + reporter.writeValue("Angl-in", tilt_output_.angle_signal_input); + reporter.writeValue("Angl-fl", tilt_output_.angle_signal_filtered); + reporter.writeValue("Angle", tilt_output_.angle); + reporter.writeValue("Fixed", static_cast(tilt_output_.is_fixed)); + } + //*** End: UpdatableState implementation ***// + + protected: + //override RotorActuator's setWrench function using TiltOutput + virtual void setWrench(Wrench& wrench) override + { + if (tilt_params_.use_simple_rotor_model) { + RotorActuator::setWrench(wrench); + } + else { + Vector3r normal = getNormal(); + wrench.force = normal * tilt_output_.rotor_output.thrust; + wrench.torque = normal * tilt_output_.rotor_output.torque_scaler; + } + } + + private: //methods + void setTiltOutput() + { + //populate rotor_output with output from RotorActuator + tilt_output_.rotor_output = RotorActuator::getOutput(); + + //if we want to use more complicated rotor model, need to modify thrust and torque outputs + if (!tilt_params_.use_simple_rotor_model) { + if (tilt_output_.rotor_output.speed > 0.0f) { + calculateThrustTorque(tilt_output_); + } + //else { + // tilt_output_.rotor_output.thrust = 0.0f; + // tilt_output_.rotor_output.torque_scaler = 0.0f; + //} + } + + tilt_output_.angle_signal_filtered = angle_signal_filter_.getOutput(); + tilt_output_.angle_signal_input = angle_signal_filter_.getInput(); + tilt_output_.angle = angle_filter_.getOutput(); + Vector3r normal = getNormal(); + tilt_output_.angle_from_vertical = VectorMath::sgn(normal(0)) * std::acos(-normal(2)); + } + + //this more complicated rotor model is necessary because it more acurately calculates the effects + //of airspeed on the thrust a rotor is able to produce. It wouldn't really matter for multirotors, + //but is very important for fixedwing vehicles which travel at high airspeeds + void calculateThrustTorque(TiltOutput& output) + { + //motor model from "Small Unmanned Aircraft: Theory and Practice", supplement + //see https://uavbook.byu.edu/lib/exe/fetch.php?media=uavbook_supplement.pdf, pg. 8 + const real_T& throttle = output.rotor_output.control_signal_filtered; + const RotorTurningDirection& turn_dir = output.rotor_output.turning_direction; + const RotorTiltableParams& p = tilt_params_; + const real_T rho = environment_->getState().air_density; + + real_T v_in = p.max_voltage * throttle; + real_T a = p.CQ0 * rho * pow(p.prop_diameter, 5) / pow(2 * M_PIf, 2); + + real_T b1 = airspeed_ * p.CQ1 * rho * pow(p.prop_diameter, 4) / (2 * M_PIf); + real_T b2 = pow(p.motor_KQ, 2) / p.motor_resistance; + real_T b = b1 + b2; + + real_T c1 = pow(airspeed_, 2) * p.CQ2 * rho * pow(p.prop_diameter, 3); + real_T c2 = -v_in * p.motor_KQ / p.motor_resistance; + real_T c3 = p.motor_KQ * p.no_load_current; + real_T c = c1 + c2 + c3; + + real_T Omega_op = (-b + sqrt(pow(b, 2) - 4 * a * c)) / (2 * a); + + real_T J_op = 2 * M_PIf * airspeed_ / (Omega_op * p.prop_diameter); + real_T CT = p.CT2 * pow(J_op, 2) + p.CT1 * J_op + p.CT0; + real_T CQ = p.CQ2 * pow(J_op, 2) + p.CQ1 * J_op + p.CQ0; + real_T n = Omega_op / (2 * M_PIf); + real_T T_p = rho * pow(n, 2) * pow(p.prop_diameter, 4) * CT; + real_T Q_p = rho * pow(n, 2) * pow(p.prop_diameter, 5) * CQ; + + output.rotor_output.thrust = T_p; + //output.rotor_output.torque_scaler = Q_p * static_cast(turn_dir); + output.rotor_output.torque_scaler = 0.0; + } + + private: //fields + Vector3r normal_nominal_; + Vector3r normal_current_; + bool is_fixed_; + Vector3r rotation_axis_; + real_T max_angle_; + RotorTiltableParams tilt_params_; + FirstOrderFilter angle_signal_filter_; //first order filter for rotor angle signal + FirstOrderFilter angle_filter_; //first order filter for actual rotor angle (much slower) + TiltOutput tilt_output_; + // Vector3r airspeed_body_vector_; + real_T airspeed_; + + const Environment* environment_ = nullptr; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RotorTiltableParams.hpp b/AirLib/include/vehicles/rov/RotorTiltableParams.hpp new file mode 100644 index 000000000..3664a85e6 --- /dev/null +++ b/AirLib/include/vehicles/rov/RotorTiltableParams.hpp @@ -0,0 +1,45 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RotorTiltableParams_hpp +#define msr_airlib_RotorTiltableParams_hpp + +#include "common/Common.hpp" +#include "vehicles/multirotor/RotorParams.hpp" + +namespace msr +{ +namespace airlib +{ + + struct RotorTiltableParams + { + + bool use_simple_rotor_model = true; //if true, will use method of Rotor to calculate thrust and torque. + //otherwise, will use more complicated model below + + RotorParams rotor_params = RotorParams(); //only some params will be used if use_simple_rotor_model is false + + real_T angle_signal_filter_tc = 0.005f; //time constant for angle command signal + real_T angle_filter_tc = 0.1f; //time constant for servo arm to reach angle command signal + + //rotor parameters + real_T max_voltage = 11.1; //nominal voltage for 3-cell battery. Could increase to 12.6 to indicate battery is fully charged... + real_T prop_diameter = 7 * (0.0254); + real_T motor_resistance = 0.3; + real_T motor_KV = 1450; + real_T motor_KQ = (1.0 / motor_KV) * 60.0 / (2.0 * M_PIf); + real_T no_load_current = 0.83; + + //prop second order coefficients + real_T CT0 = 0.1167; + real_T CT1 = 0.0144; + real_T CT2 = -0.1480; + real_T CQ0 = 0.0088; + real_T CQ1 = 0.0129; + real_T CQ2 = -0.0216; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RovBodyParams.hpp b/AirLib/include/vehicles/rov/RovBodyParams.hpp new file mode 100644 index 000000000..ee320501e --- /dev/null +++ b/AirLib/include/vehicles/rov/RovBodyParams.hpp @@ -0,0 +1,324 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_rov_body_params_hpp +#define msr_airlib_rov_body_params_hpp + +#include "common/Common.hpp" +#include "RotorTiltableParams.hpp" +#include "AeroParams.hpp" +#include "sensors/SensorCollection.hpp" +#include "sensors/SensorFactory.hpp" +#include "vehicles/rov/api/RovApiBase.hpp" +namespace msr +{ +namespace airlib +{ + + class RovBodyParams + { + //All units are SI + public: //types + struct RotorTiltableConfiguration + { + Vector3r position; //relative to center of gravity of vehicle body + Vector3r normal_nominal; + Vector3r rotation_axis; + + bool is_fixed; + real_T max_angle; + RotorTurningDirection direction; + RotorTiltableParams params; //each rotor may have different parameters if different motor types are used on different parts of the aircraft + + RotorTiltableConfiguration() + { + } + RotorTiltableConfiguration(const Vector3r& position_val, const Vector3r& normal_nominal_val, const Vector3r& rotation_axis_val, + const bool is_fixed_val, const real_T max_angle_val, const RotorTurningDirection direction_val, const RotorTiltableParams& params_val) + : position{ position_val }, normal_nominal{ normal_nominal_val }, rotation_axis{ rotation_axis_val }, is_fixed{ is_fixed_val }, max_angle{ max_angle_val }, direction{ direction_val }, params{ params_val } + { + } + }; + + struct Params + { + + Vector3r added_mass_linear; + Vector3r added_mass_angular; + Vector3r damping_linear; + Vector3r damping_angular; + Vector3r damping_linear_q; + Vector3r damping_angular_q; + + real_T off_z; + uint rotor_count; + vector rotor_configs; + real_T mass; + //vector rotor_poses; + Matrix3x3r inertia; + //Vector3r body_box; + real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. + real_T friction = 0.5f; + AeroParams aero_params; + }; + + protected: //must override by derived class + virtual void setupParams() = 0; + virtual const SensorFactory* getSensorFactory() const = 0; + + public: //interface + virtual std::unique_ptr createRovApi() = 0; + + virtual ~RovBodyParams() = default; + virtual void initialize(const AirSimSettings::VehicleSetting* vehicle_setting) + { + sensor_storage_.clear(); + sensors_.clear(); + + setupParams(); + + addSensorsFromSettings(vehicle_setting); + } + + const Params& getParams() const + { + return params_; + } + Params& getParams() + { + return params_; + } + SensorCollection& getSensors() + { + return sensors_; + } + const SensorCollection& getSensors() const + { + return sensors_; + } + + void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting) + { + const auto& sensor_settings = vehicle_setting->sensors; + + getSensorFactory()->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_); + } + + protected: + //TODO: is there a way to read params in from vehicle_setting? + /* + static void computeInertiaMatrix(Matrix3x3r& inertia, const Vector3r& body_box, const vector& rotor_poses, + real_T box_mass, real_T motor_assembly_weight) + { + inertia = Matrix3x3r::Zero(); + + //http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html + inertia(0, 0) = box_mass / 12.0f * (body_box.y() * body_box.y() + body_box.z() * body_box.z()); + inertia(1, 1) = box_mass / 12.0f * (body_box.x() * body_box.x() + body_box.z() * body_box.z()); + inertia(2, 2) = box_mass / 12.0f * (body_box.x() * body_box.x() + body_box.y() * body_box.y()); + + for (size_t i = 0; i < rotor_poses.size(); ++i) { + const auto& pos = rotor_poses.at(i).position; + inertia(0, 0) += (pos.y() * pos.y() + pos.z() * pos.z()) * motor_assembly_weight; + inertia(1, 1) += (pos.x() * pos.x() + pos.z() * pos.z()) * motor_assembly_weight; + inertia(2, 2) += (pos.x() * pos.x() + pos.y() * pos.y()) * motor_assembly_weight; + } + } + */ + void setupBlueROV2Heavy(Params& params) + { + + /* Note: rotor_poses are built in this order: + + Vertical Motors: + x-axis + (2) X | X (1) + | + -------------- y-axis + | + (3) X | X (4) + + Horizontal Motors: Note: Horiztonal motor are oriented at 45 Degress + x-axis + (6)/ | \(5) + | + -------------- y-axis + | + (7)\ | /(8) + */ + + // Define the rotors position, orientation and rotation direction + + params.rotor_count = 8; + + // A) Vertical Rotors (r1-r4) + + // r1 - front right rotor + Vector3r r1_pos(0.1, 0.1, 0.0); //position + Vector3r r1_norm(0.0, 0.0, -1.0); //direction (upwards) + Vector3r r1_rot(0.0, -1.0, 0.0); //Tilting axis (if it tilts) + bool r1_fixed = true; //No tilting + real_T r1_max = 0.0; + RotorTurningDirection r1_direction = RotorTurningDirection::RotorTurningDirectionCW; + RotorTiltableParams r1_params = RotorTiltableParams(); + r1_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r1_config(r1_pos, r1_norm, r1_rot, r1_fixed, r1_max, r1_direction, r1_params); + params.rotor_configs.push_back(r1_config); + + //r2 - front left rotor + Vector3r r2_pos(0.1, -0.1, 0.f); + Vector3r r2_norm(0.f, 0.f, -1.0); + Vector3r r2_rot(0.f, 1.f, 0.f); + bool r2_fixed = true; + real_T r2_max = 0.0; + RotorTurningDirection r2_direction = RotorTurningDirection::RotorTurningDirectionCCW; + RotorTiltableParams r2_params = RotorTiltableParams(); + r2_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r2_config(r2_pos, r2_norm, r2_rot, r2_fixed, r2_max, r2_direction, r2_params); + params.rotor_configs.push_back(r2_config); + + //r3 - rear left rotor + Vector3r r3_pos(-0.1, -0.1, 0.f); //position + Vector3r r3_norm(0.f, 0.f, -1.0); //direction (upwards) + Vector3r r3_rot(1.f, 0.f, 0.f); //Tilting axis (if it tilts) + bool r3_fixed = true; //No tilting + real_T r3_max = 0.f; + RotorTurningDirection r3_direction = RotorTurningDirection::RotorTurningDirectionCCW; + RotorTiltableParams r3_params = RotorTiltableParams(); + r3_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r3_config(r3_pos, r3_norm, r3_rot, r3_fixed, r3_max, r3_direction, r3_params); + params.rotor_configs.push_back(r3_config); + + //r4 - rear right rotor + Vector3r r4_pos(-0.1, 0.1, 0.f); + Vector3r r4_norm(0.f, 0.f, -1.0); + Vector3r r4_rot(1.f, 0.f, 0.f); //this gets ignored + bool r4_fixed = true; + real_T r4_max = 0.f; + RotorTurningDirection r4_direction = RotorTurningDirection::RotorTurningDirectionCW; + RotorTiltableParams r4_params = RotorTiltableParams(); + r4_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r4_config(r4_pos, r4_norm, r4_rot, r4_fixed, r4_max, r4_direction, r4_params); + params.rotor_configs.push_back(r4_config); + + //Horizontal rotors (r5-r6) + + //r5 - front right rotor + Vector3r r5_pos(0.1, 0.1, 0.f); + Vector3r r5_norm(0.5, -0.5, 0.0); + Vector3r r5_rot(1.f, 0.f, 0.f); //this gets ignored + bool r5_fixed = true; + real_T r5_max = 0.f; + RotorTurningDirection r5_direction = RotorTurningDirection::RotorTurningDirectionCW; + + RotorTiltableParams r5_params = RotorTiltableParams(); + r5_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r5_config(r5_pos, r5_norm, r5_rot, r5_fixed, r5_max, r5_direction, r5_params); + params.rotor_configs.push_back(r5_config); + + //r6 - front right rotor + Vector3r r6_pos(0.1, -0.1, 0.f); + Vector3r r6_norm(0.5, 0.5, 0.0); + Vector3r r6_rot(1.f, 0.f, 0.f); //this gets ignored + bool r6_fixed = true; + real_T r6_max = 0.f; + RotorTurningDirection r6_direction = RotorTurningDirection::RotorTurningDirectionCCW; + + RotorTiltableParams r6_params = RotorTiltableParams(); + r6_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r6_config(r6_pos, r6_norm, r6_rot, r6_fixed, r6_max, r6_direction, r6_params); + params.rotor_configs.push_back(r6_config); + + //r7 - front right rotor + Vector3r r7_pos(-0.1, -0.1, 0.f); + Vector3r r7_norm(-0.5, 0.5, 0.0); + Vector3r r7_rot(1.f, 0.f, 0.f); //this gets ignored + bool r7_fixed = true; + real_T r7_max = 0.f; + RotorTurningDirection r7_direction = RotorTurningDirection::RotorTurningDirectionCW; + + RotorTiltableParams r7_params = RotorTiltableParams(); + r7_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r7_config(r7_pos, r7_norm, r7_rot, r7_fixed, r7_max, r7_direction, r7_params); + params.rotor_configs.push_back(r7_config); + + //r8 - front right rotor + Vector3r r8_pos(-0.1, 0.1, 0.f); + Vector3r r8_norm(-0.5, -0.5, 0.0); + Vector3r r8_rot(1.f, 0.f, 0.f); //this gets ignored + bool r8_fixed = true; + real_T r8_max = 0.f; + RotorTurningDirection r8_direction = RotorTurningDirection::RotorTurningDirectionCCW; + + RotorTiltableParams r8_params = RotorTiltableParams(); + r8_params.rotor_params.calculateMaxThrust(); + RotorTiltableConfiguration r8_config(r8_pos, r8_norm, r8_rot, r8_fixed, r8_max, r8_direction, r8_params); + params.rotor_configs.push_back(r8_config); + + //BlueROV2 parameters + //https://flex.flinders.edu.au/file/27aa0064-9de2-441c-8a17-655405d5fc2e/1/ThesisWu2018.pdf + + real_T rov_mass = 11.5f; //Mass + params.aero_params = AeroParams(); //Aerodynamics params + + //params.off_z = 0.02; //offset in z direction between the centre of bouyancy COB and centre of mass COM + params.off_z = 0.02; + + //Hydrodynamic cofeceients + + //Added Mass Coffecients + params.added_mass_linear.x() = -5.5f; + params.added_mass_linear.y() = -12.7f; + params.added_mass_linear.z() = -14.57f; + + params.added_mass_angular.x() = -0.12f; + params.added_mass_angular.y() = -0.12f; + params.added_mass_angular.z() = -0.12f; + + params.mass = rov_mass; //Mass + + params.inertia << 0.30f, 0.0f, 0.0f, //Inertia Matrix + 0.0f, 0.30f, 0.0f, + 0.0f, 0.0f, 0.30f; + + //Damping Coffecients + params.damping_linear.x() = -4.03f; + params.damping_linear.y() = -6.22f; + params.damping_linear.z() = -5.18f; + + params.damping_angular.x() = -0.07f; + params.damping_angular.y() = -0.07f; + params.damping_angular.z() = -0.07f; + + params.damping_linear_q.x() = -18.18f; + params.damping_linear_q.y() = -21.66f; + params.damping_linear_q.z() = -36.99f; + + params.damping_angular_q.x() = -1.55f; + params.damping_angular_q.y() = -1.55f; + params.damping_angular_q.z() = -1.55f; + + /* + params.body_box.x() = 0.180f; + params.body_box.y() = 0.11f; + params.body_box.z() = 0.040f; + real_T box_mass = params.mass + real_T motor_assembly_weight = 0.052f; // weight for TBS motors + + params.rotor_poses.clear(); + for (uint i = 0; i < 4; i++) { + Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); + params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); + }; + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + */ + } + + private: + Params params_; + SensorCollection sensors_; //maintains sensor type indexed collection of sensors + vector> sensor_storage_; //RAII for created sensors + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RovParamsFactory.hpp b/AirLib/include/vehicles/rov/RovParamsFactory.hpp new file mode 100644 index 000000000..b652a53af --- /dev/null +++ b/AirLib/include/vehicles/rov/RovParamsFactory.hpp @@ -0,0 +1,43 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_vehicles_RovBodyParamsFactory_hpp +#define msr_airlib_vehicles_RovBodyParamsFactory_hpp + +#include "vehicles/rov/firmwares/rov_simple/RovSimpleParams.hpp" +#include "vehicles/rov/firmwares/mavlink/Px4RovParams.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovBodyParamsFactory + { + public: + static std::unique_ptr createConfig(const AirSimSettings::VehicleSetting* vehicle_setting, + std::shared_ptr sensor_factory) + { + std::unique_ptr config; + + if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4Rov) { + config.reset(new Px4RovParams(*static_cast(vehicle_setting), + sensor_factory)); + } + else if (vehicle_setting->vehicle_type == "" || //default config + vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeRovSimple) { + config.reset(new RovSimpleParams(vehicle_setting, sensor_factory)); + } + else + throw std::runtime_error(Utils::stringf( + "Cannot create vehicle config because vehicle name '%s' is not recognized", + vehicle_setting->vehicle_name.c_str())); + + config->initialize(vehicle_setting); + + return config; + } + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/RovPhysicsBody.hpp b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp new file mode 100644 index 000000000..d97b45ba6 --- /dev/null +++ b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp @@ -0,0 +1,241 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_rovbody_hpp +#define msr_airlib_rovbody_hpp + +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "RotorTiltable.hpp" +#include "AeroVertex.hpp" +#include "api/VehicleApiBase.hpp" +#include "api/VehicleSimApiBase.hpp" +#include "RovBodyParams.hpp" +#include "physics/PhysicsBody.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovPhysicsBody : public PhysicsBody + { + public: + RovPhysicsBody(RovBodyParams* params, VehicleApiBase* vehicle_api, + Kinematics* kinematics, Environment* environment) + : params_(params), vehicle_api_(vehicle_api) + { + setName("RovPhysicsBody"); + vehicle_api_->setParent(this); + initialize(kinematics, environment); + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override + { + //reset rotors, kinematics and environment + PhysicsBody::resetImplementation(); + + //reset sensors last after their ground truth has been reset + resetSensors(); + } + + virtual void update() override + { + //update forces on vertices that we will use next + PhysicsBody::update(); + + //Note that controller gets updated after kinematics gets updated in updateKinematics + //otherwise sensors will have values from previous cycle causing lags which will appear + //as crazy jerks whenever commands like velocity is issued + } + virtual void reportState(StateReporter& reporter) override + { + //call base + PhysicsBody::reportState(reporter); + + reportSensors(*params_, reporter); + + //report rotors + for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) { + reporter.startHeading("", 1); + reporter.writeValue("RotorTiltable", rotor_index); + reporter.endHeading(false, 1); + rotors_.at(rotor_index).reportState(reporter); + } + + //report AeroVertex + reporter.startHeading("", 1); + reporter.writeValue("AeroVertex", 0); + reporter.endHeading(false, 1); + aero_vertex_.reportState(reporter); + } + //*** End: UpdatableState implementation ***// + + //Fast Physics engine calls this method to set next kinematics + virtual void updateKinematics(const Kinematics::State& kinematics) override + { + PhysicsBody::updateKinematics(kinematics); + + updateSensorsAndController(); + } + + //External Physics engine calls this method to keep physics bodies updated and move rotors + virtual void updateKinematics() override + { + PhysicsBody::updateKinematics(); + + updateSensorsAndController(); + } + + void updateSensorsAndController() + { + updateSensors(*params_, getKinematics(), getEnvironment()); + + //update controller which will update actuator control signal + vehicle_api_->update(); + + //update all control inputs + //for now assuming actuation order is {flap1, flap2, flap3, rotor1thr, rotor1ang, rotor2thr, rotor2ang, ...} + vector rov_inputs = { 0.0, 0.0, 0.0 }; + //vector rov_inputs = { vehicle_api_->getActuation(0), vehicle_api_->getActuation(1), vehicle_api_->getActuation(2) }; + aero_vertex_.setFlapInputs(rov_inputs); + + //transfer new input values from controller to rotors + for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) { + rotors_.at(rotor_index).setControlSignal(vehicle_api_->getActuation(rotor_index)); + //rotors_.at(rotor_index).setAngleSignal(vehicle_api_->getActuation(rotor_index)); + + //rotors_.at(rotor_index).setControlSignal(vehicle_api_->getActuation(3 + 2 * rotor_index)); + //rotors_.at(rotor_index).setAngleSignal(vehicle_api_->getActuation(3 + 2 * rotor_index + 1)); + } + } + + //this gets called in getDragWrench function in physics engine + virtual void setAirspeedBody(const Vector3r airspeed_body_vector) + { + aero_vertex_.setAirspeedVertex(airspeed_body_vector); + + for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) { + rotors_[rotor_index].setAirspeedRotor(airspeed_body_vector); + } + } + + void overwriteRotorTilts(const vector& angles, bool spin_props) + { + for (uint i = 0; i < rotors_.size(); ++i) { + auto& rotor = rotors_.at(i); + rotor.overwriteTilt(angles[i], spin_props); + } + } + + //sensor getter + const SensorCollection& getSensors() const + { + return params_->getSensors(); + } + + //physics body interface + virtual uint wrenchVertexCount() const override + { + return params_->getParams().rotor_count; //+1 for aerovertex + } + virtual PhysicsBodyVertex& getWrenchVertex(uint index) override + { + + return rotors_.at(index); + } + virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const override + { + + return rotors_.at(index); + } + + virtual real_T getRestitution() const override + { + return params_->getParams().restitution; + } + virtual real_T getFriction() const override + { + return params_->getParams().friction; + } + + RotorTiltable::TiltOutput getRotorOutput(uint index) const + { + return rotors_.at(index).getOutput(); + } + + AeroVertex::Output getAeroOutput() const + { + return aero_vertex_.getOutput(); + } + + uint rotorCount() const + { + return params_->getParams().rotor_count; + } + + virtual ~RovPhysicsBody() = default; + + private: //methods + void initialize(Kinematics* kinematics, Environment* environment) + { + PhysicsBody::initialize(params_->getParams().mass, params_->getParams().inertia, kinematics, environment); + + createRotors(*params_, rotors_, environment); + createAeroVertex(*params_, aero_vertex_, environment, kinematics); + + initSensors(*params_, getKinematics(), getEnvironment()); + } + + static void createRotors(const RovBodyParams& params, vector& rotors, const Environment* environment) + { + rotors.clear(); + //for each rotor pose + for (uint rotor_index = 0; rotor_index < params.getParams().rotor_configs.size(); ++rotor_index) { + const RovBodyParams::RotorTiltableConfiguration& rotor_config = params.getParams().rotor_configs.at(rotor_index); + rotors.emplace_back(rotor_config.position, rotor_config.normal_nominal, rotor_config.direction, rotor_config.is_fixed, rotor_config.rotation_axis, rotor_config.max_angle, rotor_config.params, environment, rotor_index); + } + } + + static void createAeroVertex(const RovBodyParams& params, AeroVertex& aero_vertex, const Environment* environment, const Kinematics* kinematics) + { + AirState air_state; + air_state.zero(); + aero_vertex.initialize(params.getParams().aero_params, environment, kinematics, air_state); + } + + void reportSensors(RovBodyParams& params, StateReporter& reporter) + { + params.getSensors().reportState(reporter); + } + + void updateSensors(RovBodyParams& params, const Kinematics::State& state, const Environment& environment) + { + unused(state); + unused(environment); + params.getSensors().update(); + } + + void initSensors(RovBodyParams& params, const Kinematics::State& state, const Environment& environment) + { + params.getSensors().initialize(&state, &environment); + } + + void resetSensors() + { + params_->getSensors().reset(); + } + + private: //fields + RovBodyParams* params_; + AeroVertex aero_vertex_; //one aerovertex at the center of mass for calculating aerodynamic forces and moments + vector rotors_; + + VehicleApiBase* vehicle_api_; + }; + +} +} //namespace + +#endif diff --git a/AirLib/include/vehicles/rov/api/RovApiBase.hpp b/AirLib/include/vehicles/rov/api/RovApiBase.hpp new file mode 100644 index 000000000..cd4cdbe54 --- /dev/null +++ b/AirLib/include/vehicles/rov/api/RovApiBase.hpp @@ -0,0 +1,391 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_DroneVTOLControlServer_hpp +#define msr_airlib_DroneVTOLControlServer_hpp + +#include "common/Common.hpp" +#include "RovCommon.hpp" +#include "safety/SafetyEval.hpp" +#include "physics/Kinematics.hpp" +#include "physics/Environment.hpp" +#include "api/VehicleApiBase.hpp" + +#include +#include +#include + +namespace msr +{ +namespace airlib +{ + + class RovApiBase : public VehicleApiBase + { + + protected: //must be implemented + /************************* low level move APIs *********************************/ + virtual void commandMotorPWMs(const vector& pwm_values) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + + /************************* State APIs *********************************/ + virtual Kinematics::State getKinematicsEstimated() const = 0; + virtual LandedState getLandedState() const = 0; + virtual GeoPoint getGpsLocation() const = 0; + virtual const RovApiParams& getRovApiParams() const = 0; + + /************************* basic config APIs *********************************/ + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. + //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. + virtual float getDistanceAccuracy() const = 0; + + protected: //optional overrides but recommended, default values may work + virtual float getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; + + //below methods gets called by default implementations of move-related commands that would use a long + //running loop. These can be used by derived classes to do some init/cleanup. + virtual void beforeTask() + { + //default is do nothing + } + virtual void afterTask() + { + //default is do nothing + } + + public: //optional overrides + virtual void moveByRC(const RCData& rc_data); + + //below method exist for any firmwares that may want to use ground truth for debugging purposes + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + virtual void setCollisionInfo(const CollisionInfo& collision_info) + { + collision_info_ = collision_info; + } + + virtual void resetImplementation() override; + + public: //these APIs uses above low level APIs + virtual ~RovApiBase() = default; + + /************************* high level move APIs *********************************/ + //return value of these function is true if command was completed without interruption or timeouts + virtual bool takeoff(float timeout_sec); + virtual bool land(float timeout_sec); + virtual bool goHome(float timeout_sec); + + virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByMotorPWMs(const vector& pwm_values, float duration); + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); + virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool rotateToYaw(float yaw, float timeout_sec, float margin); + virtual bool rotateByYawRate(float yaw_rate, float duration); + virtual bool hover(); + virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + + /************************* Safety APIs *********************************/ + virtual void setSafetyEval(const shared_ptr safety_eval_ptr); + virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); + + /************************* high level status APIs *********************************/ + RotorTiltableStates getRotorStates() const + { + return rotor_states_; + } + + Kinematics::State getKinematicsGroundTruth() const + { + Kinematics::State state; + state.pose.position = kinematics_->pose.position; + state.pose.orientation = kinematics_->pose.orientation; + state.twist.linear = kinematics_->twist.linear; + state.twist.angular = kinematics_->twist.angular; + return state; + } + + RovState getRovState() const + { + RovState state; + state.kinematics_estimated = getKinematicsEstimated(); + state.kinematics_true = getKinematicsGroundTruth(); + //TODO: add GPS health, accuracy in API + state.gps_location = getGpsLocation(); + state.timestamp = clock()->nowNanos(); + state.landed_state = getLandedState(); + state.rc_data = getRCData(); + state.ready = isReady(state.ready_message); + state.can_arm = canArm(); + return state; + } + + /******************* Task management Apis ********************/ + virtual void cancelLastTask() override + { + token_.cancel(); + } + + /******************* rotors' states setter ********************/ + void setRotorStates(const RotorTiltableStates& rotor_states) + { + rotor_states_ = rotor_states; + } + + protected: //utility methods + typedef std::function WaitFunction; + + //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); + virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); + virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); + virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); + + /************* safety checks & emergency maneuvers ************/ + virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); + virtual bool safetyCheckVelocity(const Vector3r& velocity); + virtual bool safetyCheckVelocityZ(float vx, float vy, float z); + virtual bool safetyCheckDestination(const Vector3r& dest_loc); + + /************* wait helpers ************/ + // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). + // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. + Waiter waitForFunction(WaitFunction function, float max_wait); + + //useful for derived class to check after takeoff + bool waitForZ(float timeout_sec, float z, float margin); + + /************* other short hands ************/ + virtual Vector3r getPosition() const + { + return getKinematicsEstimated().pose.position; + } + virtual Vector3r getVelocity() const + { + return getKinematicsEstimated().twist.linear; + } + virtual Quaternionr getOrientation() const + { + return getKinematicsEstimated().pose.orientation; + } + + CancelToken& getCancelToken() + { + return token_; + } + + public: //types + class UnsafeMoveException : public VehicleMoveException + { + public: + const SafetyEval::EvalResult result; + + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") + : VehicleMoveException(message), result(result_val) + { + } + }; + + protected: //types + class SingleCall + { + public: + SingleCall(RovApiBase* api) + : api_(api) + { + auto& token = api->getCancelToken(); + + //if we can't get lock, cancel previous call + if (!token.try_lock()) { + //TODO: should we worry about spurious failures in try_lock? + token.cancel(); + token.lock(); + } + + if (isRootCall()) + token.reset(); + //else this is not the start of the call + } + + virtual ~SingleCall() + { + auto& token = api_->getCancelToken(); + + if (isRootCall()) + token.reset(); + //else this is not the end of the call + + token.unlock(); + } + + protected: + RovApiBase* getVehicleApi() + { + return api_; + } + + bool isRootCall() + { + return api_->getCancelToken().getRecursionCount() == 1; + } + + private: + RovApiBase* api_; + }; + + class SingleTaskCall : public SingleCall + { + public: + SingleTaskCall(RovApiBase* api) + : SingleCall(api) + { + if (isRootCall()) + api->beforeTask(); + } + + virtual ~SingleTaskCall() + { + if (isRootCall()) + getVehicleApi()->afterTask(); + } + }; + + //use this lock for vehicle status APIs + struct StatusLock + { + //this const correctness gymnastic is required because most + //status update APIs are const + StatusLock(const RovApiBase* api) + : lock_( + *const_cast(&api->status_mutex_)) + { + } + + private: + //we need mutable here because status APIs are const and shouldn't change data members + mutable std::lock_guard lock_; + }; + + private: //types + struct PathPosition + { + uint seg_index; + float offset; + Vector3r position; + }; + + struct PathSegment + { + Vector3r seg_normalized; + Vector3r seg; + float seg_length; + float seg_velocity; + float start_z; + float seg_path_length; + + PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + { + seg = end - start; + seg_length = seg.norm(); + seg_normalized = seg.normalized(); + start_z = start.z(); + seg_path_length = path_length; + + seg_velocity = velocity; + } + }; + + //RAII + class ObsStrategyChanger + { + private: + shared_ptr safety_eval_ptr_; + SafetyEval::ObsAvoidanceStrategy old_strategy_; + + public: + ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + { + safety_eval_ptr_ = safety_eval_ptr; + old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); + safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); + } + ~ObsStrategyChanger() + { + safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); + } + }; + + private: //methods + float setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); + void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); + void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); + void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); + bool isYawWithinMargin(float yaw_target, float margin) const; + + private: //variables + CancelToken token_; + std::recursive_mutex status_mutex_; + RCData rc_data_trims_; + shared_ptr safety_eval_ptr_; + float obs_avoidance_vel_ = 0.5f; + + //TODO: make this configurable? + float landing_vel_ = 0.2f; //velocity to use for landing + float approx_zero_vel_ = 0.05f; + float approx_zero_angular_vel_ = 0.01f; + RotorTiltableStates rotor_states_; + + CollisionInfo collision_info_; + const Kinematics::State* kinematics_; + const Environment* environment_; + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/api/RovCommon.hpp b/AirLib/include/vehicles/rov/api/RovCommon.hpp new file mode 100644 index 000000000..414ebdfd2 --- /dev/null +++ b/AirLib/include/vehicles/rov/api/RovCommon.hpp @@ -0,0 +1,114 @@ +#ifndef msr_airlib_DroneVTOLCommon_hpp +#define msr_airlib_DroneVTOLCommon_hpp + +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "physics/Kinematics.hpp" +#include "vehicles/multirotor/api/MultirotorCommon.hpp" + +namespace msr +{ +namespace airlib +{ + // Structs for rotor state API + struct RotorTiltableParameters + { + real_T thrust = 0; + real_T torque_scaler = 0; + real_T speed = 0; + real_T angle = 0; + + RotorTiltableParameters() + { + } + + RotorTiltableParameters(const real_T& thrust_val, const real_T& torque_scaler_val, const real_T& speed_val, const real_T& angle_val) + : thrust(thrust_val), torque_scaler(torque_scaler_val), speed(speed_val), angle(angle_val) + { + } + + void update(const real_T& thrust_val, const real_T& torque_scaler_val, const real_T& speed_val, const real_T& angle_val) + { + thrust = thrust_val; + torque_scaler = torque_scaler_val; + speed = speed_val; + angle = angle_val; + } + }; + + struct RotorTiltableStates + { + std::vector rotors; + uint64_t timestamp; + + RotorTiltableStates() + { + } + + RotorTiltableStates(const std::vector& rotors_val, uint64_t timestamp_val) + : rotors(rotors_val), timestamp(timestamp_val) + { + } + }; + + //properties of vehicle + struct RovApiParams + { + RovApiParams(){}; + //what is the breaking distance for given velocity? + //Below is just proportionality constant to convert from velocity to breaking distance + float vel_to_breaking_dist = 0.5f; //ideally this should be 2X for very high speed but for testing we are keeping it 0.5 + float min_breaking_dist = 1; //min breaking distance + float max_breaking_dist = 3; //min breaking distance + float breaking_vel = 1.0f; + float min_vel_for_breaking = 3; + + //what is the differential positional accuracy of cur_loc? + //this is not same as GPS accuracy because translational errors + //usually cancel out. Typically this would be 0.2m or less + float distance_accuracy = 0.1f; + + //what is the minimum clearance from obstacles? + float obs_clearance = 2; + + //what is the +/-window we should check on obstacle map? + //for example 2 means check from ticks -2 to 2 + int obs_window = 0; + }; + + struct RovState + { + CollisionInfo collision; + Kinematics::State kinematics_estimated; + Kinematics::State kinematics_true; + GeoPoint gps_location; + uint64_t timestamp; + LandedState landed_state; + RCData rc_data; + bool ready; // indicates drone is ready for commands + std::string ready_message; // can show error message if drone is not reachable over the network or is not responding + bool can_arm; // indicates drone is ready to be armed + + RovState() + { + } + RovState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_estimated_val, + const Kinematics::State& kinematics_true_val, const GeoPoint& gps_location_val, uint64_t timestamp_val, + LandedState landed_state_val, const RCData& rc_data_val, bool ready_val, const std::string& message, bool can_arm_val) + : collision(collision_val), kinematics_estimated(kinematics_estimated_val), kinematics_true(kinematics_true_val), gps_location(gps_location_val), timestamp(timestamp_val), landed_state(landed_state_val), rc_data(rc_data_val), ready(ready_val), ready_message(message), can_arm(can_arm_val) + { + } + + //shortcuts + const Vector3r& getPosition() const + { + return kinematics_estimated.pose.position; + } + const Quaternionr& getOrientation() const + { + return kinematics_estimated.pose.orientation; + } + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/api/RovRpcLibAdaptors.hpp b/AirLib/include/vehicles/rov/api/RovRpcLibAdaptors.hpp new file mode 100644 index 000000000..fce7ac242 --- /dev/null +++ b/AirLib/include/vehicles/rov/api/RovRpcLibAdaptors.hpp @@ -0,0 +1,129 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovRpcLibAdaptors_hpp +#define msr_airlib_RovRpcLibAdaptors_hpp + +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "api/RpcLibAdaptorsBase.hpp" +#include "vehicles/rov/api/RovCommon.hpp" +#include "vehicles/rov/api/RovApiBase.hpp" +#include "vehicles/multirotor/api/MultirotorCommon.hpp" +#include "common/ImageCaptureBase.hpp" +#include "safety/SafetyEval.hpp" + +#include "common/common_utils/WindowsApisCommonPre.hpp" +#include "rpc/msgpack.hpp" +#include "common/common_utils/WindowsApisCommonPost.hpp" + +namespace msr +{ +namespace airlib_rpclib +{ + + class RovRpcLibAdaptors : public RpcLibAdaptorsBase + { + public: + struct RotorTiltableParameters + { + msr::airlib::real_T thrust; + msr::airlib::real_T torque_scaler; + msr::airlib::real_T speed; + msr::airlib::real_T angle; + + MSGPACK_DEFINE_MAP(thrust, torque_scaler, speed, angle); + + RotorTiltableParameters() + { + } + + RotorTiltableParameters(const msr::airlib::RotorTiltableParameters& s) + { + thrust = s.thrust; + torque_scaler = s.torque_scaler; + speed = s.speed; + angle = s.angle; + } + + msr::airlib::RotorTiltableParameters to() const + { + return msr::airlib::RotorTiltableParameters(thrust, torque_scaler, speed, angle); + } + }; + + struct RotorTiltableStates + { + std::vector rotors; + uint64_t timestamp; + + MSGPACK_DEFINE_MAP(rotors, timestamp); + + RotorTiltableStates() + { + } + + RotorTiltableStates(const msr::airlib::RotorTiltableStates& s) + { + for (const auto& r : s.rotors) { + rotors.push_back(RotorTiltableParameters(r)); + } + timestamp = s.timestamp; + } + + msr::airlib::RotorTiltableStates to() const + { + std::vector d; + for (const auto& r : rotors) { + d.push_back(r.to()); + } + return msr::airlib::RotorTiltableStates(d, timestamp); + } + }; + + struct RovState + { + CollisionInfo collision; + KinematicsState kinematics_estimated; + KinematicsState kinematics_true; + GeoPoint gps_location; + msr::airlib::LandedState landed_state; + uint64_t timestamp; + RCData rc_data; + bool ready; + std::string ready_message; + std::vector controller_messages; + bool can_arm; + + MSGPACK_DEFINE_MAP(collision, kinematics_estimated, kinematics_true, gps_location, + timestamp, landed_state, rc_data, ready, ready_message, can_arm); + + RovState() + { + } + + RovState(const msr::airlib::RovState& s) + { + collision = s.collision; + kinematics_estimated = s.kinematics_estimated; + kinematics_true = s.kinematics_true; + gps_location = s.gps_location; + timestamp = s.timestamp; + landed_state = s.landed_state; + rc_data = RCData(s.rc_data); + ready = s.ready; + ready_message = s.ready_message; + can_arm = s.can_arm; + } + + msr::airlib::RovState to() const + { + return msr::airlib::RovState(collision.to(), kinematics_estimated.to(), kinematics_true.to(), gps_location.to(), timestamp, landed_state, rc_data.to(), ready, ready_message, can_arm); + } + }; + }; + +} +} //namespace + +#endif diff --git a/AirLib/include/vehicles/rov/api/RovRpcLibClient.hpp b/AirLib/include/vehicles/rov/api/RovRpcLibClient.hpp new file mode 100644 index 000000000..52bec5f9a --- /dev/null +++ b/AirLib/include/vehicles/rov/api/RovRpcLibClient.hpp @@ -0,0 +1,84 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovRpcLibClient_hpp +#define msr_airlib_RovRpcLibClient_hpp + +#include "common/Common.hpp" +#include +#include "common/CommonStructs.hpp" +#include "common/ImageCaptureBase.hpp" +#include "vehicles/rov/api/RovApiBase.hpp" +#include "api/RpcLibClientBase.hpp" +#include "vehicles/rov/api/RovCommon.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovRpcLibClient : public RpcLibClientBase + { + public: + RovRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); + + RovRpcLibClient* takeoffAsync(float timeout_sec = 20, const std::string& vehicle_name = ""); + RovRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); + RovRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); + RovRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + RovRpcLibClient* moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + RovRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByVelocityAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + RovRpcLibClient* moveByVelocityZAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + RovRpcLibClient* moveOnPathAsync(const vector& path, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + RovRpcLibClient* moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + RovRpcLibClient* moveToZAsync(float z, float velocity, float timeout_sec = Utils::max(), + const YawMode& yaw_mode = YawMode(), float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); + RovRpcLibClient* rotateToYawAsync(float yaw, float timeout_sec = Utils::max(), float margin = 5, const std::string& vehicle_name = ""); + RovRpcLibClient* rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name = ""); + RovRpcLibClient* hoverAsync(const std::string& vehicle_name = ""); + + RovRpcLibClient* setRovPose(Pose pose, const vector& tilt_angles, bool ignore_collision, const std::string& vehicle_name = ""); + RovRpcLibClient* moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name = ""); + + void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name = ""); + void moveByRC(const RCData& rc_data, const std::string& vehicle_name = ""); + + RovState getRovState(const std::string& vehicle_name = ""); + RotorTiltableStates getRotorStates(const std::string& vehicle_name = ""); + + bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = ""); + + virtual RovRpcLibClient* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan()) override; + + virtual ~RovRpcLibClient(); //required for pimpl + + private: + struct impl; + std::unique_ptr pimpl_; + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/api/RovRpcLibServer.hpp b/AirLib/include/vehicles/rov/api/RovRpcLibServer.hpp new file mode 100644 index 000000000..142d27586 --- /dev/null +++ b/AirLib/include/vehicles/rov/api/RovRpcLibServer.hpp @@ -0,0 +1,31 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovRpcLibServer_hpp +#define msr_airlib_RovRpcLibServer_hpp + +#include "common/Common.hpp" +#include +#include "api/RpcLibServerBase.hpp" +#include "vehicles/rov/api/RovApiBase.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovRpcLibServer : public RpcLibServerBase + { + public: + RovRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort); + virtual ~RovRpcLibServer(); + + protected: + virtual RovApiBase* getVehicleApi(const std::string& vehicle_name) override + { + return static_cast(RpcLibServerBase::getVehicleApi(vehicle_name)); + } + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkOutputMappings.hpp b/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkOutputMappings.hpp new file mode 100644 index 000000000..30eee7566 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkOutputMappings.hpp @@ -0,0 +1,31 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_MavLinkOutputMappings_hpp +#define msr_airlib_MavLinkOutputMappings_hpp + +#include +#include +#include + +namespace msr +{ +namespace airlib +{ + + //these vectors show how to map from Px4 actuator commands + //{Main1, Main2, ..., Main8} to the inputs that AeroBody is expecting + //-1 indicates the input is not used + //These should all be of size 3 + 2*num_rotors + struct MavLinkOutputMappings + { + std::map> mappings = { + { "Rov", { 6, 7, -1, 1, 5, 0, 4, 2, -1 } }, + { "Generic", { 1, 0, -1, 3, -1 } } //generic fixedwing + }; + }; + +} +}; //namespace + +#endif \ No newline at end of file diff --git a/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkRovApi.hpp b/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkRovApi.hpp new file mode 100644 index 000000000..476f88928 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/mavlink/MavLinkRovApi.hpp @@ -0,0 +1,2034 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_MavLinkVTOLController_hpp +#define msr_airlib_MavLinkVTOLController_hpp + +#include "MavLinkConnection.hpp" +#include "MavLinkMessages.hpp" +#include "MavLinkNode.hpp" +#include "MavLinkVehicle.hpp" +#include "MavLinkVideoStream.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/AirSimSettings.hpp" +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/PidController.hpp" +#include "common/VectorMath.hpp" +#include "common/common_utils/FileSystem.hpp" +#include "common/common_utils/SmoothingFilter.hpp" +#include "common/common_utils/Timer.hpp" +#include "physics/World.hpp" +#include "sensors/SensorCollection.hpp" +#include "vehicles/rov/api/RovApiBase.hpp" +#include "vehicles/rov/firmwares/mavlink/MavLinkOutputMappings.hpp" + +//sensors +#include "sensors/barometer/BarometerBase.hpp" +#include "sensors/distance/DistanceSimple.hpp" +#include "sensors/gps/GpsBase.hpp" +#include "sensors/imu/ImuBase.hpp" +#include "sensors/magnetometer/MagnetometerBase.hpp" + +namespace msr +{ +namespace airlib +{ + + class MavLinkRovApi : public RovApiBase + { + public: //methods + virtual ~MavLinkRovApi() + { + closeAllConnection(); + if (this->connect_thread_.joinable()) { + this->connect_thread_.join(); + } + if (this->telemetry_thread_.joinable()) { + this->telemetry_thread_.join(); + } + } + + //non-base interface specific to MavLinKDroneController + void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) + { + connection_info_ = connection_info; + sensors_ = sensors; + is_simulation_mode_ = is_simulation; + lock_step_enabled_ = connection_info.lock_step; + + MavLinkOutputMappings output_maps; + auto maps_it = output_maps.mappings.find(connection_info.model); + if (maps_it != output_maps.mappings.end()) { + actuator_mapping_ = maps_it->second; + actuator_controls_.assign(actuator_mapping_.size(), 0.0f); + } + else { + throw std::logic_error("No output mapping found for " + connection_info.model + "!"); + } + + try { + openAllConnections(); + is_ready_ = true; + } + catch (std::exception& ex) { + is_ready_ = false; + is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what()); + } + } + + Pose getMocapPose() + { + std::lock_guard guard(mocap_pose_mutex_); + return mocap_pose_; + } + + virtual const SensorCollection& getSensors() const override + { + return *sensors_; + } + + //reset PX4 stack + virtual void resetImplementation() override + { + // note this is called AFTER "initialize" when we've connected to the drone + // so this method cannot reset any of that connection state. + world_ = nullptr; + + for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) { + if (container->getName() == "World") { + // cool beans! + // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason... + world_ = static_cast(container); + } + } + RovApiBase::resetImplementation(); + was_reset_ = true; + } + + unsigned long long getSimTime() + { + // This ensures HIL_SENSOR and HIL_GPS have matching clocks. + if (lock_step_active_) { + if (sim_time_us_ == 0) { + sim_time_us_ = clock()->nowNanos() / 1000; + } + return sim_time_us_; + } + else { + return clock()->nowNanos() / 1000; + } + } + + void advanceTime() + { + sim_time_us_ = clock()->nowNanos() / 1000; + } + + //update sensors in PX4 stack + virtual void update() override + { + try { + auto now = clock()->nowNanos() / 1000; + RovApiBase::update(); + + if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_) + return; + + { + std::lock_guard guard(telemetry_mutex_); + update_count_++; + } + + if (lock_step_active_) { + if (last_update_time_ + 1000000 < now) { + // if 1 second passes then something is terribly wrong, reset lockstep mode + lock_step_active_ = false; + { + std::lock_guard guard(telemetry_mutex_); + lock_step_resets_++; + } + addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode"); + } + else if (!received_actuator_controls_) { + // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage. + return; + } + } + + last_update_time_ = now; + + { + std::lock_guard guard(telemetry_mutex_); + hil_sensor_count_++; + } + advanceTime(); + + //send sensor updates + const auto& imu_output = getImuData(""); + const auto& mag_output = getMagnetometerData(""); + const auto& baro_output = getBarometerData(""); + + sendHILSensor(imu_output.linear_acceleration, + imu_output.angular_velocity, + mag_output.magnetic_field_body, + baro_output.pressure * 0.01f /*Pa to Millibar */, + baro_output.altitude); + + sendSystemTime(); + + const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); + if (count_distance_sensors != 0) { + const auto* distance_sensor = static_cast( + sensors_->getByType(SensorBase::SensorType::Distance)); + // Don't send the data if sending to external controller is disabled in settings + if (distance_sensor && distance_sensor->getParams().external_controller) { + const auto& distance_output = distance_sensor->getOutput(); + + sendDistanceSensor(distance_output.min_distance * 100, //m -> cm + distance_output.max_distance * 100, //m -> cm + distance_output.distance * 100, //m-> cm + 0, //sensor type: //TODO: allow changing in settings? + 77, //sensor id, //TODO: should this be something real? + distance_output.relative_pose.orientation); //TODO: convert from radians to degrees? + } + } + + const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); + if (count_gps_sensors != 0) { + const auto& gps_output = getGpsData(""); + + //send GPS + if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) { + last_gps_time_ = gps_output.gnss.time_utc; + Vector3r gps_velocity = gps_output.gnss.velocity; + Vector3r gps_velocity_xy = gps_velocity; + gps_velocity_xy.z() = 0; + float gps_cog; + if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f)) + gps_cog = 0; + else + gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x())); + if (gps_cog < 0) + gps_cog += 360; + + sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog, gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10); + } + } + + auto end = clock()->nowNanos() / 1000; + { + std::lock_guard guard(telemetry_mutex_); + update_time_ += (end - now); + } + } + catch (std::exception& e) { + addStatusMessage("Exception sending messages to vehicle"); + addStatusMessage(e.what()); + disconnect(); + connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on. + } + + //must be done at the end + if (was_reset_) + was_reset_ = false; + } + + virtual bool isReady(std::string& message) const override + { + if (!is_ready_ && is_ready_message_.size() > 0) { + message = is_ready_message_; + } + return is_ready_; + } + + virtual bool canArm() const override + { + return is_ready_ && has_gps_lock_; + } + + //TODO: this method can't be const yet because it clears previous messages + virtual void getStatusMessages(std::vector& messages) override + { + updateState(); + + //clear param + messages.clear(); + + //move messages from private vars to param + std::lock_guard guard(status_text_mutex_); + while (!status_messages_.empty()) { + messages.push_back(status_messages_.front()); + status_messages_.pop(); + } + } + + virtual Kinematics::State getKinematicsEstimated() const override + { + updateState(); + Kinematics::State state; + //TODO: reduce code duplication in getPosition() etc methods? + state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); + state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); + state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); + state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate); + state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z); + //TODO: how do we get angular acceleration? + return state; + } + + virtual bool isApiControlEnabled() const override + { + return is_api_control_enabled_; + } + + virtual void enableApiControl(bool is_enabled) override + { + checkValidVehicle(); + if (is_enabled) { + mav_vehicle_->requestControl(); + is_api_control_enabled_ = true; + } + else { + mav_vehicle_->releaseControl(); + is_api_control_enabled_ = false; + } + } + + virtual Vector3r getPosition() const override + { + updateState(); + return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z); + } + + virtual Vector3r getVelocity() const override + { + updateState(); + return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z); + } + + virtual Quaternionr getOrientation() const override + { + updateState(); + return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw); + } + + virtual LandedState getLandedState() const override + { + updateState(); + return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying; + } + + virtual real_T getActuation(unsigned int actuator_index) const override + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to read motor controls while not in simulation mode"); + + std::lock_guard guard(hil_controls_mutex_); + return actuator_controls_[actuator_index]; + } + + virtual size_t getActuatorCount() const override + { + return actuator_mapping_.size(); + } + + virtual bool armDisarm(bool arm) override + { + SingleCall lock(this); + + checkValidVehicle(); + bool rc = false; + if (arm) { + float timeout_sec = 10; + waitForHomeLocation(timeout_sec); + waitForStableGroundPosition(timeout_sec); + } + + mav_vehicle_->armDisarm(arm).wait(10000, &rc); + return rc; + } + + void onArmed() + { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + if (log_ != nullptr) { + log_->close(); + log_ = nullptr; + } + + try { + std::time_t t = std::time(0); // get time now + std::tm* now = std::localtime(&t); + auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday); + auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder); + auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec); + auto fullpath = common_utils::FileSystem::combine(path, filename); + addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str())); + log_file_name_ = fullpath; + log_ = std::make_shared(); + log_->openForWriting(fullpath, false); + con->startLoggingSendMessage(log_); + con->startLoggingReceiveMessage(log_); + if (con != connection_) { + // log the SITL channel also + connection_->startLoggingSendMessage(log_); + connection_->startLoggingReceiveMessage(log_); + } + start_telemtry_thread(); + } + catch (std::exception& ex) { + addStatusMessage(std::string("Opening log file failed: ") + ex.what()); + } + } + } + } + + void onDisarmed() + { + if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) { + auto con = mav_vehicle_->getConnection(); + if (con != nullptr) { + con->stopLoggingSendMessage(); + con->stopLoggingReceiveMessage(); + } + if (connection_ != nullptr) { + connection_->stopLoggingSendMessage(); + connection_->stopLoggingReceiveMessage(); + } + } + if (log_ != nullptr) { + addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str())); + log_->close(); + log_ = nullptr; + } + } + + void waitForHomeLocation(float timeout_sec) + { + if (!current_state_.home.is_set) { + addStatusMessage("Waiting for valid GPS home location..."); + if (!waitForFunction([&]() { + return current_state_.home.is_set; + }, + timeout_sec) + .isComplete()) { + throw VehicleMoveException("Vehicle does not have a valid GPS home location"); + } + } + } + + void waitForStableGroundPosition(float timeout_sec) + { + // wait for ground stabilization + if (ground_variance_ > GroundTolerance) { + addStatusMessage("Waiting for z-position to stabilize..."); + if (!waitForFunction([&]() { + return ground_variance_ <= GroundTolerance; + }, + timeout_sec) + .isComplete()) { + auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_); + throw VehicleMoveException(msg); + } + } + } + + virtual bool takeoff(float timeout_sec) override + { + SingleCall lock(this); + + checkValidVehicle(); + + waitForHomeLocation(timeout_sec); + waitForStableGroundPosition(timeout_sec); + + bool rc = false; + auto vec = getPosition(); + auto yaw = current_state_.attitude.yaw; + float z = vec.z() + getTakeoffZ(); + if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast(timeout_sec * 1000), &rc)) { + throw VehicleMoveException("TakeOff command - timeout waiting for response"); + } + if (!rc) { + throw VehicleMoveException("TakeOff command rejected by drone"); + } + if (timeout_sec <= 0) + return true; // client doesn't want to wait. + + return waitForZ(timeout_sec, z, getDistanceAccuracy()); + } + + virtual bool land(float timeout_sec) override + { + SingleCall lock(this); + + //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now + //we assume the ground is relatively flat an we are landing roughly at the home altitude. + updateState(); + checkValidVehicle(); + if (current_state_.home.is_set) { + bool rc = false; + if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) { + throw VehicleMoveException("Landing command - timeout waiting for response from drone"); + } + else if (!rc) { + throw VehicleMoveException("Landing command rejected by drone"); + } + } + else { + throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor..."); + } + + const auto& waiter = waitForFunction([&]() { + updateState(); + return current_state_.controls.landed; + }, + timeout_sec); + + // Wait for landed state (or user cancellation) + if (!waiter.isComplete()) { + throw VehicleMoveException("Drone hasn't reported a landing state"); + } + return waiter.isComplete(); + } + + virtual bool goHome(float timeout_sec) override + { + SingleCall lock(this); + + checkValidVehicle(); + bool rc = false; + if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait( + static_cast(timeout_sec) * 1000, &rc)) { + throw VehicleMoveException("goHome - timeout waiting for response from drone"); + } + return rc; + } + + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override + { + SingleTaskCall lock(this); + + unused(adaptive_lookahead); + unused(lookahead); + unused(drivetrain); + + checkValidVehicle(); + + // save current manual, cruise, and max velocity parameters + bool result = false; + mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter; + result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter); + result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter); + result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter); + + if (result) { + // set max velocity parameter + mavlinkcom::MavLinkParameter p; + p.name = "MPC_XY_VEL_MAX"; + p.value = velocity; + mav_vehicle_->setParameter(p).wait(1000, &result); + + if (result) { + const Vector3r& goal_pos = Vector3r(x, y, z); + Vector3r goal_dist_vect; + float goal_dist; + + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + + while (!waiter.isComplete()) { + goal_dist_vect = getPosition() - goal_pos; + const Vector3r& goal_normalized = goal_dist_vect.normalized(); + goal_dist = goal_dist_vect.dot(goal_normalized); + + if (goal_dist > getDistanceAccuracy()) { + moveToPositionInternal(goal_pos, yaw_mode); + + //sleep for rest of the cycle + if (!waiter.sleep()) + return false; + } + else { + waiter.complete(); + } + } + + // reset manual, cruise, and max velocity parameters + bool result_temp = false; + mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result); + mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp); + result = result && result_temp; + mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp); + result = result && result_temp; + + return result && waiter.isComplete(); + } + } + + return result; + } + + virtual bool hover() override + { + SingleCall lock(this); + + bool rc = false; + checkValidVehicle(); + mavlinkcom::AsyncResult result = mav_vehicle_->loiter(); + //auto start_time = std::chrono::system_clock::now(); + while (!getCancelToken().isCancelled()) { + if (result.wait(100, &rc)) { + break; + } + } + return rc; + } + + virtual GeoPoint getHomeGeoPoint() const override + { + updateState(); + if (current_state_.home.is_set) + return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt); + else + return GeoPoint(Utils::nan(), Utils::nan(), Utils::nan()); + } + + virtual GeoPoint getGpsLocation() const override + { + updateState(); + return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt); + } + + virtual void sendTelemetry(float last_interval = -1) override + { + if (connection_ == nullptr || mav_vehicle_ == nullptr) { + return; + } + + // This method is called at high frequence from RovPawnSimApi::updateRendering. + mavlinkcom::MavLinkTelemetry data; + connection_->getTelemetry(data); + if (data.messages_received == 0) { + if (!hil_message_timer_.started()) { + hil_message_timer_.start(); + } + else if (hil_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); + hil_message_timer_.stop(); + } + } + else { + hil_message_timer_.stop(); + } + } + + void writeTelemetry(float last_interval = -1) + { + auto proxy = logviewer_proxy_; + auto log = log_; + + if ((logviewer_proxy_ == nullptr && log_ == nullptr)) { + return; + } + + mavlinkcom::MavLinkTelemetry data; + connection_->getTelemetry(data); + + // listen to the other mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavlinkcom::MavLinkTelemetry gcs; + mavcon->getTelemetry(gcs); + + data.handler_microseconds += gcs.handler_microseconds; + data.messages_handled += gcs.messages_handled; + data.messages_received += gcs.messages_received; + data.messages_sent += gcs.messages_sent; + + if (gcs.messages_received == 0) { + if (!gcs_message_timer_.started()) { + gcs_message_timer_.start(); + } + else if (gcs_message_timer_.seconds() > messageReceivedTimeout) { + addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again"); + } + } + else { + gcs_message_timer_.stop(); + } + } + + data.render_time = static_cast(last_interval * 1000000); // microseconds + + { + std::lock_guard guard(telemetry_mutex_); + uint32_t average_delay = 0; + uint32_t average_update = 0; + if (hil_sensor_count_ != 0) { + average_delay = actuator_delay_ / hil_sensor_count_; + average_update = static_cast(update_time_ / hil_sensor_count_); + } + + data.update_rate = update_count_; + data.sensor_rate = hil_sensor_count_; + data.actuation_delay = average_delay; + data.lock_step_resets = lock_step_resets_; + data.update_time = average_update; + // reset the counters we just captured. + update_count_ = 0; + hil_sensor_count_ = 0; + actuator_delay_ = 0; + update_time_ = 0; + } + + if (proxy != nullptr) { + proxy->sendMessage(data); + } + + if (log != nullptr) { + mavlinkcom::MavLinkMessage msg; + msg.magic = MAVLINK_STX_MAVLINK1; + data.encode(msg); + msg.update_checksum(); + // disk I/O is unpredictable, so we have to get it out of the update loop + // which is why this thread exists. + log->write(msg); + } + } + + void start_telemtry_thread() + { + if (this->telemetry_thread_.joinable()) { + this->telemetry_thread_.join(); + } + + // reset the counters we use for telemetry. + update_count_ = 0; + hil_sensor_count_ = 0; + actuator_delay_ = 0; + + this->telemetry_thread_ = std::thread(&MavLinkRovApi::telemtry_thread, this); + } + + void telemtry_thread() + { + while (log_ != nullptr) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + writeTelemetry(1); + } + } + + virtual float getCommandPeriod() const override + { + return 1.0f / 50; //1 period of 50hz + } + + virtual float getTakeoffZ() const override + { + // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe + // enough to get out of the backwash turbulence. Negative due to NED coordinate system. + return -3.0f; + } + + virtual float getDistanceAccuracy() const override + { + return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled + } + + protected: //methods + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) override + { + unused(controllerType); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandMotorPWMs(const vector& pwm_values) override + { + unused(pwm_values); + Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override + { + if (target_height_ != -z) { + // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best + // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not + // been tested on a real drone outside jMavSim, so it may need recalibrating... + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust); + } + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust); + } + + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + if (target_height_ != -z) { + thrust_controller_.setPoint(-z, .05f, .005f, 0.09f); + target_height_ = -z; + } + checkValidVehicle(); + auto state = mav_vehicle_->getVehicleState(); + float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + checkValidVehicle(); + mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle); + } + + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw); + } + + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw); + } + + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override + { + checkValidVehicle(); + float yaw = yaw_mode.yaw_or_rate * M_PIf / 180; + mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw); + } + + //TODO: decouple RovApiBase, VehicalParams and SafetyEval + virtual const RovApiParams& getRovApiParams() const override + { + //defaults are good for PX4 generic quadcopter. + static const RovApiParams vehicle_params_; + return vehicle_params_; + } + + virtual void beforeTask() override + { + startOffboardMode(); + } + + virtual void afterTask() override + { + endOffboardMode(); + } + + public: + class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog + { + public: + MavLinkLogViewerLog(std::shared_ptr proxy) + { + proxy_ = proxy; + } + + ~MavLinkLogViewerLog() + { + proxy_ = nullptr; + } + + void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override + { + if (proxy_ != nullptr) { + unused(timestamp); + mavlinkcom::MavLinkMessage copy; + ::memcpy(©, &msg, sizeof(mavlinkcom::MavLinkMessage)); + try { + proxy_->sendMessage(copy); + } + catch (std::exception&) { + failures++; + if (failures == 100) { + // hmmm, doesn't like the proxy, bad socket perhaps, so stop trying... + proxy_ = nullptr; + } + } + } + } + + private: + std::shared_ptr proxy_; + int failures = 0; + }; + + protected: //methods + virtual void connect() + { + if (!connecting_) { + connecting_ = true; + if (this->connect_thread_.joinable()) { + this->connect_thread_.join(); + } + this->connect_thread_ = std::thread(&MavLinkRovApi::connect_thread, this); + } + } + + virtual void disconnect() + { + addStatusMessage("Disconnecting mavlink vehicle"); + connected_ = false; + connecting_ = false; + + if (is_armed_) { + // close the telemetry log. + is_armed_ = false; + onDisarmed(); + } + + if (connection_ != nullptr) { + if (is_hil_mode_set_ && mav_vehicle_ != nullptr) { + setNormalMode(); + } + + connection_->stopLoggingSendMessage(); + connection_->stopLoggingReceiveMessage(); + connection_->close(); + } + + if (hil_node_ != nullptr) { + hil_node_->close(); + } + + if (mav_vehicle_ != nullptr) { + auto c = mav_vehicle_->getConnection(); + if (c != nullptr) { + c->stopLoggingSendMessage(); + c->stopLoggingReceiveMessage(); + } + mav_vehicle_->close(); + mav_vehicle_ = nullptr; + } + + if (video_server_ != nullptr) + video_server_->close(); + + if (logviewer_proxy_ != nullptr) { + logviewer_proxy_->close(); + logviewer_proxy_ = nullptr; + } + + if (logviewer_out_proxy_ != nullptr) { + logviewer_out_proxy_->close(); + logviewer_out_proxy_ = nullptr; + } + + if (qgc_proxy_ != nullptr) { + qgc_proxy_->close(); + qgc_proxy_ = nullptr; + } + + resetState(); + } + + void connect_thread() + { + addStatusMessage("Waiting for mavlink vehicle..."); + connecting_ = true; + createMavConnection(connection_info_); + if (mav_vehicle_ != nullptr) { + connectToLogViewer(); + connectToQGC(); + } + + if (connecting_) { + // only set connected if we haven't already been told to disconnect. + connecting_ = false; + connected_ = true; + } + } + + virtual void close() + { + disconnect(); + } + + void closeAllConnection() + { + close(); + } + + private: //methods + void openAllConnections() + { + Utils::log("Opening mavlink connection"); + close(); //just in case if connections were open + resetState(); //reset all variables we might have changed during last session + connect(); + } + + void getMocapPose(Vector3r& position, Quaternionr& orientation) const + { + position.x() = MocapPoseMessage.x; + position.y() = MocapPoseMessage.y; + position.z() = MocapPoseMessage.z; + orientation.w() = MocapPoseMessage.q[0]; + orientation.x() = MocapPoseMessage.q[1]; + orientation.y() = MocapPoseMessage.q[2]; + orientation.z() = MocapPoseMessage.q[3]; + } + + //TODO: this method used to send collision to external sim. Do we still need this? + void sendCollision(float normalX, float normalY, float normalZ) + { + checkValidVehicle(); + + mavlinkcom::MavLinkCollision collision{}; + collision.src = 1; //provider of data is MavLink system in id field + collision.id = mav_vehicle_->getLocalSystemId(); + collision.action = static_cast(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT); + collision.threat_level = static_cast(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE); + // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off. + collision.time_to_minimum_delta = normalX; + collision.altitude_minimum_delta = normalY; + collision.horizontal_minimum_delta = normalZ; + mav_vehicle_->sendMessage(collision); + } + + //TODO: do we still need this method? + bool hasVideoRequest() + { + mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req; + return video_server_->hasVideoRequest(image_req); + } + + //TODO: do we still need this method? + void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height) + { + const int MAVLINK_DATA_STREAM_IMG_PNG = 6; + video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0); + } + + //put PX4 in normal mode (i.e. non-simulation mode) + void setNormalMode() + { + if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) { + + // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode + std::lock_guard guard(set_mode_mutex_); + int mode = mav_vehicle_->getVehicleState().mode; + mode &= ~static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); + + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); + + is_hil_mode_set_ = false; + } + } + + //put PX4 in simulation mode + void setHILMode() + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode"); + + checkValidVehicle(); + + // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode + std::lock_guard guard(set_mode_mutex_); + int mode = mav_vehicle_->getVehicleState().mode; + mode |= static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); + + mavlinkcom::MavCmdDoSetMode cmd; + cmd.command = static_cast(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE); + cmd.Mode = static_cast(mode); + mav_vehicle_->sendCommand(cmd); + + is_hil_mode_set_ = true; + } + + bool startOffboardMode() + { + checkValidVehicle(); + try { + mav_vehicle_->requestControl(); + } + catch (std::exception& ex) { + ensureSafeMode(); + addStatusMessage(std::string("Request control failed: ") + ex.what()); + return false; + } + return true; + } + + void endOffboardMode() + { + // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth. + // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout + // when you stop sending move commands and the behavior on timeout is then determined by the drone itself. + // mav_vehicle_->releaseControl(); + ensureSafeMode(); + } + + void ensureSafeMode() + { + if (mav_vehicle_ != nullptr) { + const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState(); + if (state.controls.landed || !state.controls.armed) { + return; + } + } + } + + void checkValidVehicle() + { + if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) { + throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding"); + } + } + + //status update methods should call this first! + void updateState() const + { + StatusLock lock(this); + if (mav_vehicle_ != nullptr) { + int version = mav_vehicle_->getVehicleStateVersion(); + if (version != state_version_) { + current_state_ = mav_vehicle_->getVehicleState(); + state_version_ = version; + } + } + } + + virtual void normalizeActuatorControls() + { + //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case + //we normalize them to 0 to 1 for PX4 + if (!is_controls_0_1_) { + // change -1 to 1 to 0 to 1. + for (size_t i = 0; i < actuator_controls_.size(); ++i) { + actuator_controls_[i] = (actuator_controls_[i] + 1.0f) / 2.0f; + } + } + else { + //this applies to PX4 and may work differently on other firmwares. + //We use 0.2 as idle rotors which leaves out range of 0.8 + for (size_t i = 0; i < actuator_controls_.size(); ++i) { + actuator_controls_[i] = Utils::clip(0.8f * actuator_controls_[i] + 0.20f, 0.0f, 1.0f); + } + } + } + + bool sendTestMessage(std::shared_ptr node) + { + try { + // try and send a test message. + mavlinkcom::MavLinkHeartbeat test; + test.autopilot = static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4); + test.type = static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS); + test.base_mode = 0; + test.custom_mode = 0; + test.mavlink_version = 3; + node->sendMessage(test); + test.system_status = 0; + return true; + } + catch (std::exception&) { + return false; + } + } + + bool connectToLogViewer() + { + //set up logviewer proxy + if (connection_info_.logviewer_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, logviewer_proxy_, connection); + if (!sendTestMessage(logviewer_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_proxy_->getConnection()->close(); + logviewer_proxy_ = nullptr; + } + + std::shared_ptr out_connection; + createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, logviewer_out_proxy_, out_connection); + if (!sendTestMessage(logviewer_out_proxy_)) { + // error talking to log viewer, so don't keep trying, and close the connection also. + logviewer_out_proxy_->getConnection()->close(); + logviewer_out_proxy_ = nullptr; + } + else if (mav_vehicle_ != nullptr) { + auto proxylog = std::make_shared(logviewer_out_proxy_); + mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog); + mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog); + if (connection_ != nullptr) { + connection_->startLoggingSendMessage(proxylog); + connection_->startLoggingReceiveMessage(proxylog); + } + } + } + return logviewer_proxy_ != nullptr; + } + + bool connectToQGC() + { + if (connection_info_.qgc_ip_address.size() > 0) { + std::shared_ptr connection; + createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection); + if (!sendTestMessage(qgc_proxy_)) { + // error talking to QGC, so don't keep trying, and close the connection also. + qgc_proxy_->getConnection()->close(); + qgc_proxy_ = nullptr; + } + else { + connection->subscribe([=](std::shared_ptr connection_val, const mavlinkcom::MavLinkMessage& msg) { + unused(connection_val); + processQgcMessages(msg); + }); + } + } + return qgc_proxy_ != nullptr; + } + + void createProxy(std::string name, std::string ip, int port, string local_host_ip, + std::shared_ptr& node, std::shared_ptr& connection) + { + if (connection_ == nullptr) + throw std::domain_error("MavLinkRovApi requires connection object to be set before createProxy call"); + + connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port); + + // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint + // and all other messages are funneled through from PX4 via the Join method below. + node = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + node->connect(connection); + + // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be + // send directly to the PX4 (using whatever sysid/compid comes from that remote node). + connection_->join(connection); + + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != connection_) { + mavcon->join(connection); + } + } + + static std::string findPX4() + { + auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0); + for (auto iter = result.begin(); iter != result.end(); iter++) { + mavlinkcom::SerialPortInfo info = *iter; + if (( + (info.vid == pixhawkVendorId) && + (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId)) || + ((info.displayName.find(L"PX4_") != std::string::npos))) { + // printf("Auto Selecting COM port: %S\n", info.displayName.c_str()); + + std::string portName_str; + + for (wchar_t ch : info.portName) { + portName_str.push_back(static_cast(ch)); + } + return portName_str; + } + } + return ""; + } + + void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) + { + if (connection_info.use_serial) { + createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate); + } + else { + createMavEthernetConnection(connection_info); + } + + //Uncomment below for sending images over MavLink + //connectToVideoServer(); + } + + void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info) + { + close(); + + connecting_ = true; + is_controls_0_1_ = true; + std::string remoteIpAddr; + actuator_controls_.assign(actuator_mapping_.size(), 0.0f); + + if (connection_info.use_tcp) { + if (connection_info.tcp_port == 0) { + throw std::invalid_argument("TcpPort setting has an invalid value."); + } + + auto msg = Utils::stringf("Waiting for TCP connection on port %d, local IP %s", connection_info.tcp_port, connection_info_.local_host_ip.c_str()); + addStatusMessage(msg); + try { + connection_ = std::make_shared(); + remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port); + } + catch (std::exception& e) { + addStatusMessage("Accepting TCP socket failed, is another instance running?"); + addStatusMessage(e.what()); + return; + } + } + else if (connection_info.udp_address.size() > 0) { + if (connection_info.udp_port == 0) { + throw std::invalid_argument("UdpPort setting has an invalid value."); + } + + connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port); + } + else { + throw std::invalid_argument("Please provide valid connection info for your drone."); + } + + // start listening to the SITL connection. + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); + + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); + + if (connection_info.use_tcp) { + addStatusMessage(std::string("Connected to SITL over TCP.")); + } + else { + addStatusMessage(std::string("Connected to SITL over UDP.")); + } + + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + + if (connection_info_.control_ip_address != "") { + if (connection_info_.control_port_local == 0) { + throw std::invalid_argument("LocalControlPort setting has an invalid value."); + } + if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") { + remoteIpAddr = connection_info_.control_ip_address; + } + if (remoteIpAddr == "local" || remoteIpAddr == "localhost") { + remoteIpAddr = "127.0.0.1"; + } + + // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection. + // The HIL mavlink connection can only handle HIL_SENSOR messages. This separate channel is needed for + // everything else. + addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP %s ...", + connection_info_.control_port_local, + connection_info_.local_host_ip.c_str(), + remoteIpAddr.c_str())); + + // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ? + for (int retries = 60; retries >= 0 && connecting_; retries--) { + try { + std::shared_ptr gcsConnection; + if (remoteIpAddr == "127.0.0.1") { + gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs", + connection_info_.local_host_ip, + connection_info_.control_port_local); + } + else { + gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", + connection_info_.local_host_ip, + remoteIpAddr, + connection_info_.control_port_remote); + } + mav_vehicle_->connect(gcsConnection); + // need to try and send something to make sure the connection is good. + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + break; + } + catch (std::exception&) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + if (mav_vehicle_ == nullptr) { + // play was stopped! + return; + } + + if (mav_vehicle_->getConnection() != nullptr) { + addStatusMessage(std::string("Ground control connected over UDP.")); + } + else { + addStatusMessage(std::string("Timeout trying to connect ground control over UDP.")); + return; + } + } + try { + connectVehicle(); + } + catch (std::exception& e) { + addStatusMessage("Error connecting vehicle:"); + addStatusMessage(e.what()); + } + } + + void processControlMessages(const mavlinkcom::MavLinkMessage& msg) + { + // Utils::log(Utils::stringf("Control msg %d", msg.msgid)); + // PX4 usually sends the following on the control channel. + // If nothing is arriving here it means our control channel UDP connection isn't working. + // MAVLINK_MSG_ID_HIGHRES_IMU + // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET + // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW + // MAVLINK_MSG_ID_GPS_RAW_INT + // MAVLINK_MSG_ID_TIMESYNC + // MAVLINK_MSG_ID_ALTITUDE + // MAVLINK_MSG_ID_VFR_HUD + // MAVLINK_MSG_ID_ESTIMATOR_STATUS + // MAVLINK_MSG_ID_EXTENDED_SYS_STATE + // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN + // MAVLINK_MSG_ID_PING + // MAVLINK_MSG_ID_SYS_STATUS + // MAVLINK_MSG_ID_SYSTEM_TIME + // MAVLINK_MSG_ID_LINK_NODE_STATUS + // MAVLINK_MSG_ID_AUTOPILOT_VERSION + // MAVLINK_MSG_ID_COMMAND_ACK + // MAVLINK_MSG_ID_STATUSTEXT + // MAVLINK_MSG_ID_PARAM_VALUE + processMavMessages(msg); + } + + void connectVehicle() + { + // listen to this UDP mavlink connection also + auto mavcon = mav_vehicle_->getConnection(); + if (mavcon != nullptr && mavcon != connection_) { + mavcon->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processControlMessages(msg); + }); + } + else { + mav_vehicle_->connect(connection_); + } + + connected_ = true; + + // Also request home position messages + mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1); + + // now we can start our heartbeats. + mav_vehicle_->startHeartbeat(); + + // wait for px4 to connect so we can safely send any configured parameters + while (!send_params_ && connected_) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + if (connected_) { + sendParams(); + send_params_ = false; + } + } + + void createMavSerialConnection(const std::string& port_name, int baud_rate) + { + close(); + + connecting_ = true; + bool reported = false; + std::string port_name_auto = port_name; + while (port_name_auto == "" || port_name_auto == "*") { + port_name_auto = findPX4(); + if (port_name_auto == "") { + if (!reported) { + reported = true; + addStatusMessage("Could not detect a connected PX4 flight controller on any USB ports."); + addStatusMessage("You can specify USB port in settings.json."); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + if (port_name_auto == "") { + addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json."); + return; + } + + if (baud_rate == 0) { + addStatusMessage("Baud rate specified in settings.json is 0 which is invalid"); + return; + } + + addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate)); + reported = false; + + while (connecting_) { + try { + connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate); + connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mo-cap messages + hil_node_ = std::make_shared(connection_info_.sim_sysid, connection_info_.sim_compid); + hil_node_->connect(connection_); + addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str())); + + // start listening to the HITL connection. + connection_->subscribe([=](std::shared_ptr connection, const mavlinkcom::MavLinkMessage& msg) { + unused(connection); + processMavMessages(msg); + }); + + mav_vehicle_ = std::make_shared(connection_info_.vehicle_sysid, connection_info_.vehicle_compid); + + connectVehicle(); + return; + } + catch (std::exception& e) { + if (!reported) { + reported = true; + addStatusMessage("Error connecting to mavlink vehicle."); + addStatusMessage(e.what()); + addStatusMessage("Please check your USB port in settings.json."); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + } + + mavlinkcom::MavLinkHilSensor getLastSensorMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_sensor_message_; + } + + mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_distance_message_; + } + + mavlinkcom::MavLinkHilGps getLastGpsMessage() + { + std::lock_guard guard(last_message_mutex_); + return last_gps_message_; + } + + void sendParams() + { + // send any mavlink parameters from settings.json through to the connected vehicle. + if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) { + try { + for (auto iter : connection_info_.params) { + auto key = iter.first; + auto value = iter.second; + mavlinkcom::MavLinkParameter p; + p.name = key; + p.value = value; + bool result = false; + mav_vehicle_->setParameter(p).wait(1000, &result); + if (!result) { + Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str())); + } + } + } + catch (std::exception& ex) { + addStatusMessage("Exception sending parameters to vehicle"); + addStatusMessage(ex.what()); + } + } + } + + void setArmed(bool armed) + { + if (is_armed_) { + if (!armed) { + onDisarmed(); + } + } + else { + if (armed) { + onArmed(); + } + } + + is_armed_ = armed; + if (!armed) { + //reset motor controls + for (size_t i = 0; i < actuator_controls_.size(); ++i) { + actuator_controls_[i] = 0; + } + } + } + + void processQgcMessages(const mavlinkcom::MavLinkMessage& msg) + { + if (msg.msgid == MocapPoseMessage.msgid) { + std::lock_guard guard(mocap_pose_mutex_); + MocapPoseMessage.decode(msg); + getMocapPose(mocap_pose_.position, mocap_pose_.orientation); + } + //else ignore message + } + + void addStatusMessage(const std::string& message) + { + if (message.size() != 0) { + Utils::log(message); + std::lock_guard guard_status(status_text_mutex_); + //if queue became too large, clear it first + if (status_messages_.size() > status_messages_MaxSize) + Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size()); + status_messages_.push(message); + } + } + + void handleLockStep() + { + received_actuator_controls_ = true; + // if the timestamps match then it means we are in lockstep mode. + if (!lock_step_active_ && lock_step_enabled_) { + // && (HilActuatorControlsMessage.flags & 0x1)) // todo: enable this check when this flag is widely available... + if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) { + addStatusMessage("Enabling lockstep mode"); + lock_step_active_ = true; + } + } + else { + auto now = clock()->nowNanos() / 1000; + auto delay = static_cast(now - last_update_time_); + + std::lock_guard guard(telemetry_mutex_); + actuator_delay_ += delay; + } + if (world_ != nullptr) { + world_->pause(false); + } + } + + void processMavMessages(const mavlinkcom::MavLinkMessage& msg) + { + if (msg.msgid == HeartbeatMessage.msgid) { + std::lock_guard guard_heartbeat(heartbeat_mutex_); + + HeartbeatMessage.decode(msg); + + bool armed = (HeartbeatMessage.base_mode & static_cast(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0; + setArmed(armed); + if (!got_first_heartbeat_) { + Utils::log("received first heartbeat"); + + got_first_heartbeat_ = true; + if (HeartbeatMessage.autopilot == static_cast(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) && + HeartbeatMessage.type == static_cast(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) { + // PX4 will scale fixed wing servo outputs to -1 to 1 + // and it scales multi rotor servo output to 0 to 1. + is_controls_0_1_ = false; + } + } + else if (is_simulation_mode_ && !is_hil_mode_set_) { + setHILMode(); + } + } + else if (msg.msgid == StatusTextMessage.msgid) { + StatusTextMessage.decode(msg); + //lock is established by below method + addStatusMessage(std::string(StatusTextMessage.text)); + } + else if (msg.msgid == CommandLongMessage.msgid) { + CommandLongMessage.decode(msg); + if (CommandLongMessage.command == static_cast(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) { + int msg_id = static_cast(CommandLongMessage.param1 + 0.5); + if (msg_id == 115) { //HIL_STATE_QUATERNION + hil_state_freq_ = static_cast(CommandLongMessage.param2 + 0.5); + } + } + } + else if (msg.msgid == HilControlsMessage.msgid) { + throw std::invalid_argument("HilControlsMessage not supported in MavLinkRovApi"); + } + else if (msg.msgid == HilActuatorControlsMessage.msgid) { + actuators_message_supported_ = true; + + std::lock_guard guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl + + HilActuatorControlsMessage.decode(msg); + bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0; + for (size_t i = 0; i < actuator_mapping_.size(); ++i) { + if (isarmed && (actuator_mapping_[i] != -1)) { + actuator_controls_[i] = HilActuatorControlsMessage.controls[actuator_mapping_[i]]; + } + else { + actuator_controls_[i] = 0; + } + } + // if (isarmed) { + // normalizeActuatorControls(); //don't do this. Don't want to mess with controls from mavlink + // } + + handleLockStep(); + } + else if (msg.msgid == MavLinkGpsRawInt.msgid) { + MavLinkGpsRawInt.decode(msg); + auto fix_type = static_cast(MavLinkGpsRawInt.fix_type); + auto locked = (fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_GPS && + fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_FIX); + if (locked && !has_gps_lock_) { + addStatusMessage("Got GPS lock"); + has_gps_lock_ = true; + } + if (!has_home_ && current_state_.home.is_set) { + addStatusMessage("Got GPS Home Location"); + has_home_ = true; + } + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) { + // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff. + if (current_state_.controls.landed) { + monitorGroundAltitude(); + } + } + else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) { + // check landed state. + getLandedState(); + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) { + mavlinkcom::MavLinkHomePosition home; + home.decode(msg); + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) { + // this is a good time to send the params + send_params_ = true; + } + else { + // creates too much log output, only do this when debugging this issue specifically. + // Utils::log(Utils::stringf("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid)); + } + } + + void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt) + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode"); + + mavlinkcom::MavLinkHilSensor hil_sensor; + hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime(); + + hil_sensor.xacc = acceleration.x(); + hil_sensor.yacc = acceleration.y(); + hil_sensor.zacc = acceleration.z(); + hil_sensor.fields_updated = 0b111; // Set accel bit fields + + hil_sensor.xgyro = gyro.x(); + hil_sensor.ygyro = gyro.y(); + hil_sensor.zgyro = gyro.z(); + + hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields + + hil_sensor.xmag = mag.x(); + hil_sensor.ymag = mag.y(); + hil_sensor.zmag = mag.z(); + + hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields + + hil_sensor.abs_pressure = abs_pressure; + hil_sensor.pressure_alt = pressure_alt; + + hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields + + // hil_sensor.diff_pressure = diff_pressure; + + //TODO: enable temperature? diff_pressure + if (was_reset_) { + hil_sensor.fields_updated = static_cast(1 << 31); + } + + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_sensor); + received_actuator_controls_ = false; + if (lock_step_active_ && world_ != nullptr) { + world_->pauseForTime(1); // 1 second delay max waiting for actuator controls. + } + } + + std::lock_guard guard(last_message_mutex_); + last_sensor_message_ = hil_sensor; + } + + void sendSystemTime() + { + // SYSTEM TIME from host + auto tu = getSimTime(); + uint32_t ms = (uint32_t)(tu / 1000); + if (ms != last_sys_time_) { + last_sys_time_ = ms; + mavlinkcom::MavLinkSystemTime msg_system_time; + msg_system_time.time_unix_usec = tu; + msg_system_time.time_boot_ms = last_sys_time_; + if (hil_node_ != nullptr) { + hil_node_->sendMessage(msg_system_time); + } + } + } + + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, + uint8_t sensor_type, uint8_t sensor_id, const Quaternionr& orientation) + { + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); + + mavlinkcom::MavLinkDistanceSensor distance_sensor; + distance_sensor.time_boot_ms = static_cast(getSimTime() / 1000); + distance_sensor.min_distance = static_cast(min_distance); + distance_sensor.max_distance = static_cast(max_distance); + distance_sensor.current_distance = static_cast(current_distance); + distance_sensor.type = sensor_type; + distance_sensor.id = sensor_id; + + // Use custom orientation + distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM + distance_sensor.quaternion[0] = orientation.w(); + distance_sensor.quaternion[1] = orientation.x(); + distance_sensor.quaternion[2] = orientation.y(); + distance_sensor.quaternion[3] = orientation.z(); + + //TODO: use covariance parameter? + // PX4 doesn't support receiving simulated distance sensor messages this way. + // It results in lots of error messages from PX4. This code is still useful in that + // it sets last_distance_message_ and that is returned via Python API. + // + // if (hil_node_ != nullptr) { + // hil_node_->sendMessage(distance_sensor); + // } + + std::lock_guard guard(last_message_mutex_); + last_distance_message_ = distance_sensor; + } + + void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog, + float eph, float epv, int fix_type, unsigned int satellites_visible) + { + unused(satellites_visible); + + if (!is_simulation_mode_) + throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode"); + + mavlinkcom::MavLinkHilGps hil_gps; + hil_gps.time_usec = getSimTime(); + hil_gps.lat = static_cast(geo_point.latitude * 1E7); + hil_gps.lon = static_cast(geo_point.longitude * 1E7); + hil_gps.alt = static_cast(geo_point.altitude * 1000); + hil_gps.vn = static_cast(velocity.x() * 100); + hil_gps.ve = static_cast(velocity.y() * 100); + hil_gps.vd = static_cast(velocity.z() * 100); + hil_gps.eph = static_cast(eph * 100); + hil_gps.epv = static_cast(epv * 100); + hil_gps.fix_type = static_cast(fix_type); + hil_gps.vel = static_cast(velocity_xy * 100); + hil_gps.cog = static_cast(cog * 100); + hil_gps.satellites_visible = static_cast(15); + + if (hil_node_ != nullptr) { + hil_node_->sendMessage(hil_gps); + } + + if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) { + //Utils::DebugBreak(); + Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError); + } + + std::lock_guard guard(last_message_mutex_); + last_gps_message_ = hil_gps; + } + + void resetState() + { + //reset state + is_hil_mode_set_ = false; + is_controls_0_1_ = true; + hil_state_freq_ = -1; + actuators_message_supported_ = false; + state_version_ = 0; + current_state_ = mavlinkcom::VehicleState(); + target_height_ = 0; + got_first_heartbeat_ = false; + is_armed_ = false; + has_home_ = false; + sim_time_us_ = 0; + last_sys_time_ = 0; + last_gps_time_ = 0; + last_update_time_ = 0; + last_hil_sensor_time_ = 0; + update_count_ = 0; + hil_sensor_count_ = 0; + lock_step_resets_ = 0; + actuator_delay_ = 0; + is_api_control_enabled_ = false; + thrust_controller_ = PidController(); + actuator_controls_.assign(actuator_mapping_.size(), 0.0f); + was_reset_ = false; + received_actuator_controls_ = false; + lock_step_active_ = false; + lock_step_enabled_ = false; + has_gps_lock_ = false; + send_params_ = false; + mocap_pose_ = Pose::nanPose(); + ground_variance_ = 1; + ground_filter_.initialize(25, 0.1f); + cancelLastTask(); + } + + void monitorGroundAltitude() + { + // used to ensure stable altitude before takeoff. + auto position = getPosition(); + auto result = ground_filter_.filter(position.z()); + auto variance = std::get<1>(result); + if (variance >= 0) { // filter returns -1 if we don't have enough data yet. + ground_variance_ = variance; + } + } + + protected: //variables + //TODO: below was made protected from private to support Ardupilot + //implementation but we need to review this and avoid having protected variables + + const SensorCollection* sensors_; + mutable std::mutex hil_controls_mutex_; + AirSimSettings::MavLinkConnectionInfo connection_info_; + vector actuator_controls_; + vector actuator_mapping_; + bool is_simulation_mode_; + + private: //variables + static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow + static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board + static const int pixhawkFMUV5ProductId = 50; ///< Product ID for Pixhawk V5 board + static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards + static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board + static const int messageReceivedTimeout = 10; ///< Seconds + + std::shared_ptr logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_; + + size_t status_messages_MaxSize = 5000; + + std::shared_ptr hil_node_; + std::shared_ptr connection_; + std::shared_ptr video_server_; + std::shared_ptr mav_vehicle_control_; + + mavlinkcom::MavLinkAttPosMocap MocapPoseMessage; + mavlinkcom::MavLinkHeartbeat HeartbeatMessage; + mavlinkcom::MavLinkSetMode SetModeMessage; + mavlinkcom::MavLinkStatustext StatusTextMessage; + mavlinkcom::MavLinkHilControls HilControlsMessage; + mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage; + mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt; + mavlinkcom::MavLinkCommandLong CommandLongMessage; + mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed; + + mavlinkcom::MavLinkHilSensor last_sensor_message_; + mavlinkcom::MavLinkDistanceSensor last_distance_message_; + mavlinkcom::MavLinkHilGps last_gps_message_; + + std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_; + + //variables required for VehicleApiBase implementation + bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false; + bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1? + bool send_params_ = false; + std::queue status_messages_; + int hil_state_freq_; + bool actuators_message_supported_ = false; + bool was_reset_ = false; + bool has_home_ = false; + bool is_ready_ = false; + bool has_gps_lock_ = false; + bool lock_step_active_ = false; + bool lock_step_enabled_ = false; + bool received_actuator_controls_ = false; + std::string is_ready_message_; + Pose mocap_pose_; + std::thread connect_thread_; + bool connecting_ = false; + bool connected_ = false; + common_utils::SmoothingFilter ground_filter_; + double ground_variance_ = 1; + const double GroundTolerance = 0.1; + + // variables for throttling HIL_SENSOR and SYSTEM_TIME messages + uint32_t last_sys_time_ = 0; + unsigned long long sim_time_us_ = 0; + uint64_t last_gps_time_ = 0; + uint64_t last_update_time_ = 0; + uint64_t last_hil_sensor_time_ = 0; + + // variables accumulated for MavLinkTelemetry messages. + uint64_t update_time_ = 0; + uint32_t update_count_ = 0; + uint32_t hil_sensor_count_ = 0; + uint32_t lock_step_resets_ = 0; + uint32_t actuator_delay_ = 0; + std::thread telemetry_thread_; + + //additional variables required for RovApiBase implementation + //this is optional for methods that might not use vehicle commands + std::shared_ptr mav_vehicle_; + float target_height_; + bool is_api_control_enabled_; + PidController thrust_controller_; + common_utils::Timer hil_message_timer_; + common_utils::Timer gcs_message_timer_; + std::shared_ptr log_; + std::string log_file_name_; + World* world_; + + //every time we return status update, we need to check if we have new data + //this is why below two variables are marked as mutable + mutable int state_version_; + mutable mavlinkcom::VehicleState current_state_; + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/mavlink/Px4RovParams.hpp b/AirLib/include/vehicles/rov/firmwares/mavlink/Px4RovParams.hpp new file mode 100644 index 000000000..b0d3f6a09 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/mavlink/Px4RovParams.hpp @@ -0,0 +1,74 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_vehicles_Px4Rov_hpp +#define msr_airlib_vehicles_Px4Rov_hpp + +#include "vehicles/rov/firmwares/mavlink/MavLinkRovApi.hpp" +#include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" +#include "vehicles/rov/RovBodyParams.hpp" + +namespace msr +{ +namespace airlib +{ + + class Px4RovParams : public RovBodyParams + { + public: + Px4RovParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, std::shared_ptr sensor_factory) + : sensor_factory_(sensor_factory) + { + connection_info_ = getConnectionInfo(vehicle_setting); + } + + virtual ~Px4RovParams() = default; + + virtual std::unique_ptr createRovApi() override + { + unique_ptr api(new MavLinkRovApi()); + auto api_ptr = static_cast(api.get()); + api_ptr->initialize(connection_info_, &getSensors(), true); + + return api; + } + + virtual void setupParams() override + { + auto& params = getParams(); + + if (connection_info_.model == "TriRov") { + setupBlueROV2Heavy(params); + } + else if (connection_info_.model == "QuadPlane") { + setupQuadPlane(params); + } + //else //Generic + // setupGenericFixedWing(params); + } + + protected: + virtual const SensorFactory* getSensorFactory() const override + { + return sensor_factory_.get(); + } + + private: + void setupQuadPlane(Params& params) + { + //TODO + throw std::logic_error{ "QuadPlane params not yet implemented." }; + } + static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting) + { + return vehicle_setting.connection_info; + } + + private: + AirSimSettings::MavLinkConnectionInfo connection_info_; + std::shared_ptr sensor_factory_; + }; +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleApi.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleApi.hpp new file mode 100644 index 000000000..0397909db --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleApi.hpp @@ -0,0 +1,500 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovSimpleController_hpp +#define msr_airlib_RovSimpleController_hpp + +#include "vehicles/rov/api/RovApiBase.hpp" +#include "sensors/SensorCollection.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" +#include "vehicles/rov/RovBodyParams.hpp" +#include "common/Common.hpp" +#include "firmware/Firmware.hpp" +#include "RovSimpleBoard.hpp" +#include "RovSimpleCommLink.hpp" +#include "RovSimpleEstimator.hpp" +#include "RovSimpleCommon.hpp" +#include "physics/PhysicsBody.hpp" +#include "common/AirSimSettings.hpp" + +//TODO: we need to protect contention between physics thread and API server thread + +namespace msr +{ +namespace airlib +{ + + class RovSimpleApi : public RovApiBase + { + + public: + RovSimpleApi(const RovBodyParams* vehicle_params, const AirSimSettings::VehicleSetting* vehicle_setting) + : vehicle_params_(vehicle_params) + { + readSettings(*vehicle_setting); + + //TODO: set below properly for better high speed safety + safety_params_.vel_to_breaking_dist = safety_params_.min_breaking_dist = 0; + + //create sim implementations of board and commlink + board_.reset(new RovSimpleBoard(¶ms_)); + comm_link_.reset(new RovSimpleCommLink()); + estimator_.reset(new RovSimpleEstimator()); + + //create firmware + firmware_.reset(new Rov_simple::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get())); + } + + public: //VehicleApiBase implementation + virtual void resetImplementation() override + { + RovApiBase::resetImplementation(); + + firmware_->reset(); + } + virtual void update() override + { + RovApiBase::update(); + + //update controller which will update actuator control signal + firmware_->update(); + } + virtual bool isApiControlEnabled() const override + { + return firmware_->offboardApi().hasApiControl(); + } + virtual void enableApiControl(bool is_enabled) override + { + if (is_enabled) { + //comm_link should print message so no extra handling for errors + std::string message; + firmware_->offboardApi().requestApiControl(message); + } + else + firmware_->offboardApi().releaseApiControl(); + } + virtual bool armDisarm(bool arm) override + { + std::string message; + if (arm) + return firmware_->offboardApi().arm(message); + else + return firmware_->offboardApi().disarm(message); + } + virtual GeoPoint getHomeGeoPoint() const override + { + return RovSimpleCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); + } + virtual void getStatusMessages(std::vector& messages) override + { + comm_link_->getStatusMessages(messages); + } + + virtual const SensorCollection& getSensors() const override + { + return vehicle_params_->getSensors(); + } + + public: //RovApiBase implementation + virtual real_T getActuation(unsigned int actuator_index) const override + { + auto control_signal = board_->getActuatorControlSignal(actuator_index); + return control_signal; + } + virtual size_t getActuatorCount() const override + { + return vehicle_params_->getParams().rotor_count; //2 inputs for each motor, 3 control flaps + } + virtual void moveByRC(const RCData& rc_data) override + { + setRCData(rc_data); + } + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override + { + RovApiBase::setSimulatedGroundTruth(kinematics, environment); + + board_->setGroundTruthKinematics(kinematics); + estimator_->setGroundTruthKinematics(kinematics, environment); + } + virtual bool setRCData(const RCData& rc_data) override + { + last_rcData_ = rc_data; + if (rc_data.is_valid) { + board_->setIsRcConnected(true); + board_->setInputChannel(0, rc_data.roll); //X + board_->setInputChannel(1, rc_data.yaw); //Y + board_->setInputChannel(2, rc_data.throttle); //F + board_->setInputChannel(3, -rc_data.pitch); //Z + board_->setInputChannel(4, static_cast(rc_data.getSwitch(0))); //angle rate or level + board_->setInputChannel(5, static_cast(rc_data.getSwitch(1))); //Allow API control + board_->setInputChannel(6, static_cast(rc_data.getSwitch(2))); + board_->setInputChannel(7, static_cast(rc_data.getSwitch(3))); + board_->setInputChannel(8, static_cast(rc_data.getSwitch(4))); + board_->setInputChannel(9, static_cast(rc_data.getSwitch(5))); + board_->setInputChannel(10, static_cast(rc_data.getSwitch(6))); + board_->setInputChannel(11, static_cast(rc_data.getSwitch(7))); + } + else { //else we don't have RC data + board_->setIsRcConnected(false); + } + + return true; + } + + protected: + virtual Kinematics::State getKinematicsEstimated() const override + { + return RovSimpleCommon::toKinematicsState3r(firmware_->offboardApi().getStateEstimator().getKinematicsEstimated()); + } + + virtual Vector3r getPosition() const override + { + const auto& val = firmware_->offboardApi().getStateEstimator().getPosition(); + return RovSimpleCommon::toVector3r(val); + } + + virtual Vector3r getVelocity() const override + { + const auto& val = firmware_->offboardApi().getStateEstimator().getLinearVelocity(); + return RovSimpleCommon::toVector3r(val); + } + + virtual Quaternionr getOrientation() const override + { + const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation(); + return RovSimpleCommon::toQuaternion(val); + } + + virtual LandedState getLandedState() const override + { + return firmware_->offboardApi().getLandedState() ? LandedState::Landed : LandedState::Flying; + } + + virtual RCData getRCData() const override + { + //return what we received last time through setRCData + return last_rcData_; + } + + virtual GeoPoint getGpsLocation() const override + { + return RovSimpleCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint()); + } + + virtual float getCommandPeriod() const override + { + return 1.0f / 50; //50hz + } + + virtual float getTakeoffZ() const override + { + // pick a number, 3 meters is probably safe + // enough to get out of the backwash turbulence. Negative due to NED coordinate system. + return params_.takeoff.takeoff_z; + } + + virtual float getDistanceAccuracy() const override + { + return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled + } + + virtual void commandMotorPWMs(const vector& pwm_values) override + { + //Utils::log(Utils::stringf("commandMotorPWMs %f, %f, %f, %f", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough, GoalModeType::Passthrough); + + // Rov_simple::Axis4r goal(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + + //pwm_values is a vector of numbers between 0 and 1 for motors and between -1 and 1 for servos + firmware_->overrideActuatorOutputs(pwm_values); + } + + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawZ %f, %f, %f, %f", pitch, roll, z, yaw)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); + + // Rov_simple::Axis4r goal(roll, pitch, yaw, z); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll); + unused(pitch); + unused(yaw); + unused(z); + Utils::log("Not Implemented: commandRollPitchYawZ", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::Passthrough); + + // Rov_simple::Axis4r goal(roll, pitch, yaw, throttle); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll); + unused(pitch); + unused(yaw); + unused(throttle); + Utils::log("Not Implemented: commandRollPitchYawThrottle", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw, throttle)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + + // Rov_simple::Axis4r goal(roll, pitch, yaw_rate, throttle); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll); + unused(pitch); + unused(yaw_rate); + unused(throttle); + Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo); + } + + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + // Rov_simple::Axis4r goal(roll, pitch, yaw_rate, z); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll); + unused(pitch); + unused(yaw_rate); + unused(z); + Utils::log("Not Implemented: commandRollPitchYawrateZ", Utils::kLogLevelInfo); + } + + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::PositionWorld); + + // Rov_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, z); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll_rate); + unused(pitch_rate); + unused(yaw_rate); + unused(z); + Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo); + } + + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override + { + //Utils::log(Utils::stringf("commandRollPitchYawThrottle %f, %f, %f, %f", roll, pitch, yaw_rate, throttle)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough); + + // Rov_simple::Axis4r goal(roll_rate, pitch_rate, yaw_rate, throttle); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(roll_rate); + unused(pitch_rate); + unused(yaw_rate); + unused(throttle); + Utils::log("Not Implemented: commandAngleRatesThrottle", Utils::kLogLevelInfo); + } + + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override + { + //Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + // GoalModeType::VelocityWorld); + + // Rov_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), vz); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(vx); + unused(vy); + unused(vz); + unused(yaw_mode); + Utils::log("Not Implemented: commandVelocity", Utils::kLogLevelInfo); + } + + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override + { + //Utils::log(Utils::stringf("commandVelocityZ %f, %f, %f, %f", vx, vy, z, yaw_mode.yaw_or_rate)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, + // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + // GoalModeType::PositionWorld); + + // Rov_simple::Axis4r goal(vy, vx, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(vx); + unused(vy); + unused(z); + unused(yaw_mode); + Utils::log("Not Implemented: commandVelocityZ", Utils::kLogLevelInfo); + } + + virtual void setControllerGains(uint8_t controller_type, const vector& kp, const vector& ki, const vector& kd) override + { + // Rov_simple::GoalModeType controller_type_enum = static_cast(controller_type); + + // vector kp_axis4(4); + // vector ki_axis4(4); + // vector kd_axis4(4); + + // switch(controller_type_enum) { + // // roll gain, pitch gain, yaw gain, and no gains in throttle / z axis + // case Rov_simple::GoalModeType::AngleRate: + // kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + // ki_axis4 ={ki[0], ki[1], ki[2], 0.0}; + // kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + // params_.angle_rate_pid.p.setValues(kp_axis4); + // params_.angle_rate_pid.i.setValues(ki_axis4); + // params_.angle_rate_pid.d.setValues(kd_axis4); + // params_.gains_changed = true; + // break; + // case Rov_simple::GoalModeType::AngleLevel: + // kp_axis4 = {kp[0], kp[1], kp[2], 1.0}; + // ki_axis4 = {ki[0], ki[1], ki[2], 0.0}; + // kd_axis4 = {kd[0], kd[1], kd[2], 0.0}; + // params_.angle_level_pid.p.setValues(kp_axis4); + // params_.angle_level_pid.i.setValues(ki_axis4); + // params_.angle_level_pid.d.setValues(kd_axis4); + // params_.gains_changed = true; + // break; + // case Rov_simple::GoalModeType::VelocityWorld: + // kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + // ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + // kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + // params_.velocity_pid.p.setValues(kp_axis4); + // params_.velocity_pid.i.setValues(ki_axis4); + // params_.velocity_pid.d.setValues(kd_axis4); + // params_.gains_changed = true; + // break; + // case Rov_simple::GoalModeType::PositionWorld: + // kp_axis4 = {kp[1], kp[0], 0.0, kp[2]}; + // ki_axis4 = {ki[1], ki[0], 0.0, ki[2]}; + // kd_axis4 = {kd[1], kd[0], 0.0, kd[2]}; + // params_.position_pid.p.setValues(kp_axis4); + // params_.position_pid.i.setValues(ki_axis4); + // params_.position_pid.d.setValues(kd_axis4); + // params_.gains_changed = true; + // break; + // default: + // Utils::log("Unimplemented controller type"); + // break; + // } + unused(controller_type); + unused(kp); + unused(ki); + unused(kd); + Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo); + } + + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override + { + //Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); + + // typedef Rov_simple::GoalModeType GoalModeType; + // Rov_simple::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, + // yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, + // GoalModeType::PositionWorld); + + // Rov_simple::Axis4r goal(y, x, Utils::degreesToRadians(yaw_mode.yaw_or_rate), z); + + // std::string message; + // firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + unused(x); + unused(y); + unused(z); + unused(yaw_mode); + Utils::log("Not Implemented: commandPosition", Utils::kLogLevelInfo); + } + + virtual const RovApiParams& getRovApiParams() const override + { + return safety_params_; + } + + //*** End: RovApiBase implementation ***// + + private: + //convert pitch, roll, yaw from -1 to 1 to PWM + static uint16_t angleToPwm(float angle) + { + return static_cast(angle * 500.0f + 1500.0f); + } + //static uint16_t thrustToPwm(float thrust) + // { + // return static_cast((thrust < 0 ? 0 : thrust) * 1000.0f + 1000.0f); + // } + + static uint16_t thrustToPwm(float thrust) + { + return static_cast((thrust < 0 ? 0 : thrust) * 1000.0f + 1000.0f); + } + static uint16_t switchTopwm(float switchVal, uint maxSwitchVal = 1) + { + return static_cast(1000.0f * switchVal / maxSwitchVal + 1000.0f); + } + + void readSettings(const AirSimSettings::VehicleSetting& vehicle_setting) + { + params_.default_vehicle_state = Rov_simple::VehicleState::fromString( + vehicle_setting.default_vehicle_state == "" ? "Armed" : vehicle_setting.default_vehicle_state); + + remote_control_id_ = vehicle_setting.rc.remote_control_id; + params_.rc.allow_api_when_disconnected = vehicle_setting.rc.allow_api_when_disconnected; + params_.rc.allow_api_always = vehicle_setting.allow_api_always; + + params_.actuator.actuator_count = getActuatorCount(); + } + + private: + const RovBodyParams* vehicle_params_; + + int remote_control_id_ = 0; + Rov_simple::Params params_; + + unique_ptr board_; + unique_ptr comm_link_; + unique_ptr estimator_; + unique_ptr firmware_; + + RovApiParams safety_params_; + + RCData last_rcData_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleBoard.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleBoard.hpp new file mode 100644 index 000000000..f407fed83 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleBoard.hpp @@ -0,0 +1,170 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovSimpleBoard_hpp +#define msr_airlib_RovSimpleBoard_hpp + +#include +#include +#include "firmware/interfaces/IBoard.hpp" +#include "firmware/Params.hpp" +#include "common/Common.hpp" +#include "common/ClockFactory.hpp" +#include "physics/Kinematics.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovSimpleBoard : public Rov_simple::IBoard + { + public: + RovSimpleBoard(const Rov_simple::Params* params) + : params_(params), is_connected_(false), kinematics_(nullptr) + { + actuator_output_.assign(params_->actuator.actuator_count, 0.0f); + input_channels_.assign(params_->rc.channel_count, 0.0f); + } + + //interface for simulator -------------------------------------------------------------------------------- + //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) + void setGroundTruthKinematics(const Kinematics::State* kinematics) + { + kinematics_ = kinematics; + } + + //called to get o/p motor signal as float value + real_T getActuatorControlSignal(uint index) const + { + if (index >= actuator_output_.size()) + return 0.0f; + //convert PWM to scaled 0 to 1 control signal + return static_cast(actuator_output_[index]); + } + + //set current RC stick status + void setInputChannel(uint index, real_T val) + { + if (index >= input_channels_.size()) + input_channels_.resize(index + 1, 0.0f); + input_channels_[index] = static_cast(val); + } + + void setIsRcConnected(bool is_connected) + { + is_connected_ = is_connected; + } + + public: + //Board interface implementation -------------------------------------------------------------------------- + + virtual uint64_t micros() const override + { + return clock()->nowNanos() / 1000; + } + + virtual uint64_t millis() const override + { + return clock()->nowNanos() / 1000000; + } + + virtual float readChannel(uint16_t index) const override + { + if (index >= input_channels_.size()) + return 0.0f; + return input_channels_[index]; + } + + virtual float getAvgMotorOutput() const override + { + if (actuator_output_.empty()) + return 0.0f; + float sum_throttle = 0.0f; + for (float val : actuator_output_) { + sum_throttle += val; + } + return sum_throttle / static_cast(actuator_output_.size()); + } + + virtual bool isRcConnected() const override + { + return is_connected_; + } + + virtual void writeOutput(uint16_t index, float value) override + { + if (index >= actuator_output_.size()) { + actuator_output_.resize(index + 1, 0.0f); + } + actuator_output_[index] = value; + } + + virtual void setLed(uint8_t index, int32_t color) override + { + //TODO: implement this + unused(index); + unused(color); + } + + virtual void readAccel(float accel[3]) const override + { + const auto& linear_accel = VectorMath::transformToBodyFrame(kinematics_->accelerations.linear, kinematics_->pose.orientation); + accel[0] = linear_accel.x(); + accel[1] = linear_accel.y(); + accel[2] = linear_accel.z(); + } + + virtual void readGyro(float gyro[3]) const override + { + const auto angular_vel = kinematics_->twist.angular; //angular velocity is already in body frame + gyro[0] = angular_vel.x(); + gyro[1] = angular_vel.y(); + gyro[2] = angular_vel.z(); + } + + virtual void reset() override + { + IBoard::reset(); + + actuator_output_.assign(params_->actuator.actuator_count, 0); + input_channels_.assign(params_->rc.channel_count, 0); + is_connected_ = false; + } + + virtual void update() override + { + IBoard::update(); + + //no op for now + } + + private: + void sleep(double msec) + { + clock()->sleep_for(msec * 1000.0); + } + + const ClockBase* clock() const + { + return ClockFactory::get(); + } + + ClockBase* clock() + { + return ClockFactory::get(); + } + + private: + //motor outputs + std::vector actuator_output_; + std::vector input_channels_; + bool is_connected_; + + const Rov_simple::Params* params_; + const Kinematics::State* kinematics_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommLink.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommLink.hpp new file mode 100644 index 000000000..52fe41b38 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommLink.hpp @@ -0,0 +1,54 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovSimpleCommLink_hpp +#define msr_airlib_RovSimpleCommLink_hpp + +#include +#include "firmware/interfaces/ICommLink.hpp" +#include "common/Common.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovSimpleCommLink : public Rov_simple::ICommLink + { + public: // derived class specific methods + void getStatusMessages(std::vector& messages) + { + if (messages_.size() > 0) { + messages.insert(messages.end(), messages_.begin(), messages_.end()); + messages_.clear(); + } + } + + public: // implement CommLink interface + virtual void reset() + { + Rov_simple::ICommLink::reset(); + + messages_.clear(); + } + + virtual void update() + { + Rov_simple::ICommLink::update(); + } + + virtual void log(const std::string& message, int32_t log_level = ICommLink::kLogLevelInfo) + { + unused(log_level); + //if (log_level > 0) + // Utils::DebugBreak(); + messages_.push_back(std::string(message)); + } + + private: + std::vector messages_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommon.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommon.hpp new file mode 100644 index 000000000..f84a08cc8 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleCommon.hpp @@ -0,0 +1,105 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovSimpleCommon_hpp +#define msr_airlib_RovSimpleCommon_hpp + +#include "physics/Kinematics.hpp" +#include "common/Common.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovSimpleCommon + { + public: + static Rov_simple::Axis3r toAxis3r(const Vector3r& vec) + { + Rov_simple::Axis3r conv; + conv.x() = vec.x(); + conv.y() = vec.y(); + conv.z() = vec.z(); + + return conv; + } + + static Vector3r toVector3r(const Rov_simple::Axis3r& vec) + { + Vector3r conv; + conv.x() = vec.x(); + conv.y() = vec.y(); + conv.z() = vec.z(); + return conv; + } + + static Kinematics::State toKinematicsState3r(const Rov_simple::KinematicsState& state) + { + Kinematics::State state3r; + state3r.pose.position = toVector3r(state.position); + state3r.pose.orientation = toQuaternion(state.orientation); + state3r.twist.linear = toVector3r(state.linear_velocity); + state3r.twist.angular = toVector3r(state.angular_velocity); + state3r.accelerations.linear = toVector3r(state.linear_acceleration); + state3r.accelerations.angular = toVector3r(state.angular_acceleration); + + return state3r; + } + + static Rov_simple::Axis4r toAxis4r(const Quaternionr& q) + { + Rov_simple::Axis4r conv; + conv.x() = q.x(); + conv.y() = q.y(); + conv.z() = q.z(); + conv.val4() = q.w(); + + return conv; + } + + static Quaternionr toQuaternion(const Rov_simple::Axis4r& q) + { + Quaternionr conv; + conv.x() = q.x(); + conv.y() = q.y(); + conv.z() = q.z(); + conv.w() = q.val4(); + return conv; + } + + static Rov_simple::GeoPoint toSimpleFlightGeoPoint(const GeoPoint& geo_point) + { + Rov_simple::GeoPoint conv; + conv.latitude = geo_point.latitude; + conv.longitude = geo_point.longitude; + conv.altiude = geo_point.altitude; + + return conv; + } + + static GeoPoint toGeoPoint(const Rov_simple::GeoPoint& geo_point) + { + GeoPoint conv; + conv.latitude = geo_point.latitude; + conv.longitude = geo_point.longitude; + conv.altitude = geo_point.altiude; + + return conv; + } + + template + const T& makeConstant(T& _) + { + return const_cast(_); + } + template + T& makeVariable(const T& _) + { + return const_cast(_); + } + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleEstimator.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleEstimator.hpp new file mode 100644 index 000000000..ef279b39f --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleEstimator.hpp @@ -0,0 +1,107 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_RovSimpleEstimator_hpp +#define msr_airlib_RovSimpleEstimator_hpp + +#include "firmware/interfaces/CommonStructs.hpp" +#include "RovSimpleCommon.hpp" +#include "physics/Kinematics.hpp" +#include "physics/Environment.hpp" +#include "common/Common.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovSimpleEstimator : public Rov_simple::IStateEstimator + { + public: + virtual ~RovSimpleEstimator() {} + + //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) + void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + virtual Rov_simple::Axis3r getAngles() const override + { + Rov_simple::Axis3r angles; + VectorMath::toEulerianAngle(kinematics_->pose.orientation, + angles.pitch(), + angles.roll(), + angles.yaw()); + + //Utils::log(Utils::stringf("Ang Est:\t(%f, %f, %f)", angles.pitch(), angles.roll(), angles.yaw())); + + return angles; + } + + virtual Rov_simple::Axis3r getAngularVelocity() const override + { + const auto& anguler = kinematics_->twist.angular; + + Rov_simple::Axis3r conv; + conv.x() = anguler.x(); + conv.y() = anguler.y(); + conv.z() = anguler.z(); + + return conv; + } + + virtual Rov_simple::Axis3r getPosition() const override + { + return RovSimpleCommon::toAxis3r(kinematics_->pose.position); + } + + virtual Rov_simple::Axis3r transformToBodyFrame(const Rov_simple::Axis3r& world_frame_val) const override + { + const Vector3r& vec = RovSimpleCommon::toVector3r(world_frame_val); + const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); + return RovSimpleCommon::toAxis3r(trans); + } + + virtual Rov_simple::Axis3r getLinearVelocity() const override + { + return RovSimpleCommon::toAxis3r(kinematics_->twist.linear); + } + + virtual Rov_simple::Axis4r getOrientation() const override + { + return RovSimpleCommon::toAxis4r(kinematics_->pose.orientation); + } + + virtual Rov_simple::GeoPoint getGeoPoint() const override + { + return RovSimpleCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); + } + + virtual Rov_simple::GeoPoint getHomeGeoPoint() const override + { + return RovSimpleCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint()); + } + + virtual Rov_simple::KinematicsState getKinematicsEstimated() const override + { + Rov_simple::KinematicsState state; + state.position = getPosition(); + state.orientation = getOrientation(); + state.linear_velocity = getLinearVelocity(); + state.angular_velocity = getAngularVelocity(); + state.linear_acceleration = RovSimpleCommon::toAxis3r(kinematics_->accelerations.linear); + state.angular_acceleration = RovSimpleCommon::toAxis3r(kinematics_->accelerations.angular); + + return state; + } + + private: + const Kinematics::State* kinematics_; + const Environment* environment_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleParams.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleParams.hpp new file mode 100644 index 000000000..1042cf355 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/RovSimpleParams.hpp @@ -0,0 +1,49 @@ +#ifndef msr_airlib_vehicles_RovSimple_hpp +#define msr_airlib_vehicles_RovSimple_hpp + +#include "vehicles/rov/firmwares/rov_simple/RovSimpleApi.hpp" +#include "vehicles/rov/RovBodyParams.hpp" +#include "common/AirSimSettings.hpp" +#include "sensors/SensorFactory.hpp" + +namespace msr +{ +namespace airlib +{ + + class RovSimpleParams : public RovBodyParams + { + public: + RovSimpleParams(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory) + : vehicle_setting_(vehicle_setting), sensor_factory_(sensor_factory) + { + } + + virtual ~RovSimpleParams() = default; + + virtual std::unique_ptr createRovApi() override + { + return std::unique_ptr(new RovSimpleApi(this, vehicle_setting_)); + } + + protected: + virtual void setupParams() override + { + auto& params = getParams(); + setupBlueROV2Heavy(params); + } + + virtual const SensorFactory* getSensorFactory() const override + { + return sensor_factory_.get(); + } + + private: + vector> sensor_storage_; + const AirSimSettings::VehicleSetting* vehicle_setting_; //store as pointer because of derived classes + std::shared_ptr sensor_factory_; + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AdaptiveController.hpp new file mode 100644 index 000000000..ef1af8776 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AdaptiveController.hpp @@ -0,0 +1,675 @@ +/* +// This subroutine has been coded by: +// Mr. C. Domann +// Advanced Control Systems Lab +// The University of Oklahoma +// Advisor: Dr. A. L'Afflitto +*/ + +// -------------------------------------------------------------------------- +// DEFINES (drone based defines for physical characteristics) +// -------------------------------------------------------------------------- + +#ifndef msr_airsim_RovAdaptiveController_hpp +#define msr_airsim_RovAdaptiveController_hpp + +namespace Rov_simple +{ + +class AdaptiveController : public IController +{ +public: + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + goal_ = goal; + state_estimator_ = state_estimator; + } + + virtual const Axis4r& getOutput() override + { + // Create vectors to store state variables + Axis3r angle = 0; + Axis3r position = 0; + Axis3r angle_rate = 0; + Axis3r velocity = 0; + + // Assign state variables to placeholder variables + angle = state_estimator_->getAngles(); + angle_rate = state_estimator_->getAngularVelocity(); + position = state_estimator_->getPosition(); + velocity = state_estimator_->getLinearVelocity(); + + // Send state variables to the adaptive controller + update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); + + update_goals(); + + run(); + + // Replace PID control variables with Adaptive control variables + output_controls_[0] = get_U1(); + output_controls_[1] = get_U2(); + output_controls_[2] = get_U3(); + output_controls_[3] = get_U4(); + + return output_controls_; + } + + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } + +private: + const IBoardClock* clock_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + Axis4r output_controls_; + + //inertia parameters + const double Ix = 0.02f; + const double Iy = 0.01f; + const double Iz = 0.03f; + + const double l = 0.11f; //arm length, can make more accurate by being specific about lx, ly + const double m = 0.916f; // mass in kg + const double grav = 9.81f; // gravity + const double Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value + + // Static Gain Variables + const double k_phi = -16.75f; // roll angle //16.75 + const double k_theta = -26.75f; // pitch angle //26.75 + const double k_psi = -1.0f; // yaw angle //13 + const double k_roll = -450.0f; // roll rate //450 + const double k_pitch = -450.0f; // pitch rate //450 + const double k_yaw = -40000.0f; // yaw rate //400 + + //input saturation + const double U1_sat = 1.0f; + const double U2_sat = .95f; + const double U3_sat = .95f; + const double U4_sat = .95f; + + //trajectory parameters + const double pi = 3.14159265359f; + const double period = 45.0f; + const double radius = 2.5f; // input radius of the circle + const double alt = 5.0f; // height used for circle/square + + // other constants + const double NEQN = 7.0f; + + bool reset = true; + double x_0[12]; + GoalMode last_mode_; + bool is_last_goal_mode_all_passthrough_; + //double error[3] = { 0 }; + double ref_vec[10][3] = { { 0 } }; + double ref_sum[3] = { 0 }; + double velocity_integrator[3] = { 0 }; + static constexpr int array_length = 7; + double zero[array_length] = { 0 }; + double* adaptive_y = zero; + double* adaptive_output = zero; + double last_yaw = 0.0f; + + //********************** SlidingModeModel Variables ******************************************/ + + // state values + double x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; + + double x_des; + double y_des; + + // State Vector + double x[12][1]; + double reference[12] = { 0 }; + + // References and trajectory values + double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs + double size_square = 4; // one side of the square is size_square/2 + double r[4][1]; // z, phi, theta, psi references into controller + + // update for angle states in SlidingModeModel + double PQR[3][1], Angles[3][1], Angular_rates[3][1]; + double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc + double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters + double S3_P, S3_Q, S3_R; //error in body frame angular rates + + // Iconfig Adaptive Sliding Variables + double S2_phi, S2_theta, S2_psi; // error in euler angles + double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration + double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles + double R_matrix[3][3], R_inverse[3][3]; + + // Iconfig Sliding Variables + double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control + + Axis4r U_vec = 0; + + double U1, U2, U3, U4; + + void update_state_vals(double x_val, double vx, double y_val, double vy, double z_val, double vz, double roll, double roll_rate, double pitch, double pitch_rate, double yaw, double yaw_rate) + { + x_in = x_val; + xdot_in = vx; + y_in = -y_val; + ydot_in = -vy; + z_in = -z_val; + zdot_in = -vz; + phi_in = roll; + P_in = roll_rate; + theta_in = -pitch; + Q_in = -pitch_rate; + psi_in = yaw; + R_in = yaw_rate; + + // bias modification for level imu implementing deadband + + if (abs(phi_in) <= 0.0001) + phi_in = 0; + + if (abs(theta_in) <= 0.00001) + theta_in = 0; + + if (abs(psi_in) <= 0.0001) + psi_in = 0; + } + + void update_goals() + { + const auto& mode = goal_->getGoalMode(); + const auto& value = goal_->getGoalValue(); + + for (int i = 0; i < 12; i++) { + reference[i] = 10001.0f; + } + int count1 = 0, count2 = 0, count3 = 0, count4 = 0; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + switch (mode[axis]) { + case GoalModeType::AngleRate: + + switch (count1) { + case 0: + reference[11] = value[axis]; + break; + case 1: + reference[9] = value[axis]; + break; + case 2: + reference[10] = value[axis]; + break; + } + count1++; + break; + case GoalModeType::AngleLevel: + + switch (count2) { + case 0: + if (axis == 2) { + reference[8] = value[axis]; + } + else { + reference[6] = value[axis]; + } + break; + case 1: + reference[7] = -value[axis]; + break; + case 2: + reference[8] = value[axis]; + break; + } + count2++; + break; + case GoalModeType::VelocityWorld: + + switch (count3) { + case 0: + reference[3] = value[axis]; + break; + case 1: + reference[4] = value[axis]; + break; + case 2: + reference[5] = value[axis]; + break; + } + count3++; + break; + case GoalModeType::PositionWorld: + + switch (count4) { + case 0: + if (axis != 0) { + reference[2] = value[axis]; + } + else { + reference[0] = value[axis]; + } + break; + case 1: + reference[1] = value[axis]; + break; + case 2: + reference[2] = value[axis]; + break; + } + count4++; + break; + default: + + break; + } + if (mode[axis] != last_mode_[axis]) { + reset = true; + } + last_mode_[axis] = mode[axis]; + } + if (reference[8] < 10000) { + last_yaw = reference[8]; + } + } + + void run() + { + rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); + } + + float get_U1() //pitch + { + return static_cast(U1); + } + + float get_U2() //roll + { + return static_cast(U2); + } + + float get_U3() //thrust + { + return static_cast(U3); + } + + float get_U4() //yaw + { + return static_cast(U4); + } + + void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) + { + double Fz, xddot, yddot; + int i; + Fz = (zddot + 9.81) * m; + xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); + yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); + if (Fz == 0) { + Fz = 9.81 * m; + } + result[0][0] = (xddot * sin(yaw_current) - yddot * cos(yaw_current)) * m / Fz; + result[1][0] = (xddot * cos(yaw_current) + yddot * sin(yaw_current)) * m / Fz; + // Limit the angle reference values + for (i = 0; i < 2; i++) { + if (result[i][0] > 0.3) { + result[i][0] = 0.3; + } + else if (result[i][0] < -0.3) { + result[i][0] = -0.3; + } + } + } + + void inverse_3x3(double A[3][3], double result[3][3]) + { + double det_A; // dummy variable + det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2]) * A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + if (det_A == 0) { + result[0][0] = 0; + result[0][1] = 0; + result[0][2] = 0; + result[1][0] = 0; + result[1][1] = 0; + result[1][2] = 0; + result[2][0] = 0; + result[2][1] = 0; + result[2][2] = 0; + } + else { + result[0][0] = (1 / det_A) * (A[1][1] * A[2][2] - A[1][2] * A[2][1]); + result[0][1] = (1 / det_A) * (A[0][2] * A[2][1] - A[0][1] * A[2][2]); + result[0][2] = (1 / det_A) * (A[0][1] * A[1][2] - A[0][2] * A[1][1]); + result[1][0] = (1 / det_A) * (A[1][2] * A[2][0] - A[1][0] * A[2][2]); + result[1][1] = (1 / det_A) * (A[0][0] * A[2][2] - A[0][2] * A[2][0]); + result[1][2] = (1 / det_A) * (A[0][2] * A[1][0] - A[0][0] * A[1][2]); + result[2][0] = (1 / det_A) * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + result[2][1] = (1 / det_A) * (A[0][1] * A[2][0] - A[0][0] * A[2][1]); + result[2][2] = (1 / det_A) * (A[0][0] * A[1][1] - A[0][1] * A[1][0]); + } + } + + void PQR_generation(double states[12][1], double result[3][1]) + { + result[0][0] = states[7][0] - sin(states[6][0]) * states[10][0]; + result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0]) * sin(states[6][0])); + result[2][0] = states[11][0] * (cos(states[8][0]) * cos(states[6][0])) - states[9][0] * sin(states[6][0]); + } + + void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) + { + result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0]) * tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0]) * tan(Angles_val[1][0]); + result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); + result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); + } + + // --------------------------------------------------------------------------- + // Local Support Functions + // --------------------------------------------------------------------------- + + uint64_t getTimeU() + { + uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); + return last_time_; + } + + void remapU(double control_u1, double control_u2, double control_u3, double control_u4) + { + // Map to px4 U outputs + U1 = control_u2; // roll + U2 = -control_u3; // pitch + U3 = control_u4; // yaw + U4 = control_u1; // throttle + U_vec[0] = static_cast(U1); + U_vec[1] = static_cast(U2); + U_vec[2] = static_cast(U3); + U_vec[3] = static_cast(U4); + } + + void rungeKutta(double* y, double* yp, uint64_t t_val, double dt, int size) + { + double zero_vec[array_length] = { 0 }; + double k1[array_length] = { 0 }; + double k2[array_length] = { 0 }; + double k3[array_length] = { 0 }; + double k4[array_length] = { 0 }; + double* y_temp; + double* y_out = zero_vec; + y = yp; + y_temp = y; + model(y_temp, static_cast(t_val), y_out); + for (int n = 0; n < size; n++) { + k1[n] = dt * y_out[n]; + y_temp[n] = y[n] + k1[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { + k2[n] = dt * y_out[n]; + y_temp[n] = y[n] + k2[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { + k3[n] = dt * y_out[n]; + y_temp[n] = y[n] + k3[n]; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { + k4[n] = dt * y_out[n]; + yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; + } + } + + // --------------------------------------------------------------------------- + // SlidingModeModel method + // --------------------------------------------------------------------------- + void model(double* y, double last_time_, double* y_out) + { + unused(last_time_); + + /********** Input the State Vector***********/ + x[0][0] = x_in; + x[1][0] = xdot_in; + x[2][0] = y_in; + x[3][0] = ydot_in; + x[4][0] = z_in; + x[5][0] = zdot_in; + x[6][0] = phi_in; + PQR[0][0] = P_in; + x[8][0] = theta_in; + PQR[1][0] = Q_in; + x[10][0] = -psi_in; + PQR[2][0] = R_in; + + if (reset) { + for (int i = 0; i < 12; i++) { + x_0[i] = x[i][0]; + } + velocity_integrator[2] = 0.0f; + reset = false; + } + + double velocity_real[3] = { x[1][0], x[3][0], x[5][0] }; + double velocity_goal[3] = { reference[4], reference[3], reference[5] }; + + for (int i = 0; i < 3; i++) { + ref_sum[i] = 0; + for (int j = 0; j < 9; j++) { + ref_vec[j][i] = ref_vec[j + 1][i]; + } + if (abs(velocity_goal[i]) < 10000) { + if (i == 0) { + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.004f; + ref_vec[9][i] = -x[2 * i][0] - velocity_integrator[i]; + } + if (i == 1) { + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0007f; + ref_vec[9][i] = x[2 * i][0] - velocity_integrator[i]; + } + if (i == 2) { + velocity_integrator[i] += (-velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0005f; + ref_vec[9][i] = (x[2 * i][0] + velocity_integrator[i]); + } + } + if (abs(velocity_goal[i]) > 10000) { + ref_vec[9][i] = reference[i]; + if (i == 0) { + ref_vec[9][0] = reference[1]; + } + else if (i == 1) { + ref_vec[9][1] = reference[0]; + } + if (i == 2) { + ref_vec[9][i] = -ref_vec[9][i]; + } + if (abs(ref_vec[9][i]) > 10000) { + ref_vec[9][i] = x[2 * i][0]; + if (i == 0) { + ref_vec[9][i] = x[0][0]; + } + if (i == 1) { + ref_vec[9][i] = x[2][0]; + } + } + } + + if (i > 0) { + ref_vec[9][i] = -ref_vec[9][i]; + } + for (int j = 0; j < 10; j++) { + ref_sum[i] += ref_vec[j][i]; + } + ref_sum[i] = ref_sum[i] /= 10; //ref_vec[9][i];// + } + + x_des = ref_sum[0]; + if (x_des > 10000) { + x_des = x[0][0]; + } + + y_des = ref_sum[1]; + if (y_des > 10000) { + y_des = x[2][0]; + } + + r[0][0] = -ref_sum[2]; + if (abs(r[0][0]) > 10000) { + r[0][0] = x[4][0]; + } + + r[3][0] = reference[8]; + if (r[3][0] > 10000) { + r[3][0] = -last_yaw; + } + + double lambda_theta = 0.5f; + double lambda_theta_rate = 0.5f; + double lambda_phi = 0.95f; + double lambda_phi_rate = 0.95f; + double lambda_psi = 0.0004f; + double lambda_psi_rate = 0.05f; + double lambda_z = 0.5f; + + Angular_velocities_from_PQR(PQR, Angles, Angular_rates); + x[7][0] = Angular_rates[0][0]; + x[9][0] = Angular_rates[1][0]; + x[11][0] = Angular_rates[2][0]; + + // -9, -21 are adjustable gains for the z controller + zdotdot = -9 * x[5][0] - 21 * (x[4][0] - r[0][0]); + + /**********Iconfig Sliding Mode***********************/ + + Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs); + ref_angles[0][0] = refs[0][0]; + ref_angles[1][0] = refs[1][0]; + + /* Reference angles to be sent to inner loop */ + r[1][0] = ref_angles[0][0]; //phi ref + if (reference[6] < 10000 && reference[6] != 0.0f) { + r[1][0] = reference[6]; + } + r[2][0] = ref_angles[1][0]; //theta ref + if (reference[7] < 10000 && reference[7] != 0.0f) { + r[2][0] = reference[7]; + } + + /************ Iconfig Control Law *********************/ + // First get integrated uncertainty parameters + delta_z = y[6]; + delta_phi = y[0]; + delta_theta = y[1]; + delta_psi = y[2]; + delta_roll = y[3]; + delta_pitch = y[4]; + delta_yaw = y[5]; + + U1 = (zdotdot + grav) * m + delta_z; + + y_out[6] = lambda_z * zdotdot; // generate sliding surface in z, .015 is adjustable slope for sliding surface + + // error in euler angles + S2_phi = x[6][0] - r[1][0]; + S2_theta = x[8][0] - r[2][0]; + S2_psi = -x[10][0] - r[3][0]; + if (S2_psi > M_PI) { + S2_psi -= 2 * M_PI; + } + if (S2_psi < -M_PI) { + S2_psi += 2 * M_PI; + } + + // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired + y_out[0] = lambda_phi * S2_phi; + y_out[1] = lambda_theta * S2_theta; + y_out[2] = lambda_psi * S2_psi; + + R_matrix[0][0] = 1; + R_matrix[1][0] = sin(x[6][0] * tan(x[8][0])); + R_matrix[2][0] = cos(x[6][0]) * tan(x[8][0]); + R_matrix[1][0] = 0; + R_matrix[1][1] = cos(x[6][0]); + R_matrix[1][2] = -1 * sin(x[6][0]); + R_matrix[2][0] = 0; + R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]); + R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]); + + inverse_3x3(R_matrix, R_inverse); + rollrate_ref = R_inverse[0][0] * S2_phi * k_phi + R_inverse[0][1] * S2_theta * k_theta + R_inverse[0][2] * S2_psi * k_psi - delta_phi; + pitchrate_ref = R_inverse[1][0] * S2_phi * k_phi + R_inverse[1][1] * S2_theta * k_theta + R_inverse[1][2] * S2_psi * k_psi - delta_theta; + yawrate_ref = R_inverse[2][0] * S2_phi * k_phi + R_inverse[2][1] * S2_theta * k_theta + R_inverse[2][2] * S2_psi * k_psi - delta_psi; + + if (reference[9] < 10000 && reference[9] != 0.0f) { + rollrate_ref = reference[9]; + } + if (reference[10] < 10000 && reference[10] != 0.0f) { + pitchrate_ref = reference[10]; + } + if (reference[11] < 10000) { + yawrate_ref = reference[11]; + } + + PQR_generation(x, PQR); + S3_P = PQR[0][0] - rollrate_ref; + S3_Q = PQR[1][0] - pitchrate_ref; + S3_R = PQR[2][0] - yawrate_ref; + + // Sliding surface for the body frame angular rates to be integrated + y_out[3] = lambda_phi_rate * S3_P; + y_out[4] = lambda_theta_rate * S3_Q; + y_out[5] = lambda_psi_rate * S3_R; + + // Calculate controls for roll, pitch, and yaw + U2 = k_roll * S3_P * Ix + (Iz - Iy) * PQR[1][0] * PQR[2][0] - delta_roll; + U3 = k_pitch * S3_Q * Iy + (Ix - Iz) * PQR[0][0] * PQR[2][0] - delta_pitch; + U4 = k_yaw * S3_R * Iz + (Iy - Ix) * PQR[0][0] * PQR[1][0] - delta_yaw; + + // Rescale such that the outputs normalize from -1,1 + + U1 = U1 / 80; //sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal + + U2 = U2 / 80; + + U3 = U3 / 80; + + U4 = U4 / 80; + + // Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2 + if (U1 > 1) { + U1 = 1; + } + else if (U1 < 0.35) { + U1 = 0.35; + } + if (U2 > U2_sat) { + U2 = U2_sat; + } + else if (U2 < -U2_sat) { + U2 = -U2_sat; + } + if (U3 > U3_sat) { + U3 = U3_sat; + } + else if (U3 < -U3_sat) { + U3 = -U3_sat; + } + if (U4 > U4_sat) { + U4 = U4_sat; + } + else if (U4 < -U4_sat) { + U4 = -U4_sat; + } + + remapU(U1, U2, U3, U4); //remap to axis4r + + } // SlidingModeModel */ +}; + +} +#endif diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleLevelController.hpp new file mode 100644 index 000000000..c5f384890 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleLevelController.hpp @@ -0,0 +1,143 @@ + +#pragma once + +#include "interfaces/IUpdatable.hpp" +#include "interfaces/IGoal.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IAxisController.hpp" +#include "AngleRateController.hpp" +#include "Params.hpp" +#include "PidController.hpp" +#include "common/common_utils/Utils.hpp" +#include +#include + +namespace Rov_simple +{ + +class AngleLevelController : public IAxisController + , + public IGoal //for internal rate controller +{ +public: + AngleLevelController(Params* params, const IBoardClock* clock = nullptr) + : params_(params), clock_(clock) + { + } + + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + if (axis > 2) + throw std::invalid_argument("AngleLevelController only supports axis 0-2 but it was " + std::to_string(axis)); + + axis_ = axis; + goal_ = goal; + state_estimator_ = state_estimator; + + //initialize level PID + pid_.reset(new PidController(clock_, + PidConfig(params_->angle_level_pid.p[axis], params_->angle_level_pid.i[axis], params_->angle_level_pid.d[axis]))); + + //initialize rate controller + rate_controller_.reset(new AngleRateController(params_, clock_)); + rate_controller_->initialize(axis, this, state_estimator_); + + //we will be setting goal for rate controller so we need these two things + rate_mode_ = GoalMode::getUnknown(); + rate_mode_[axis] = GoalModeType::AngleRate; + } + + virtual void reset() override + { + IAxisController::reset(); + + pid_->reset(); + rate_controller_->reset(); + rate_goal_ = Axis4r(); + output_ = TReal(); + } + + virtual void update() override + { + IAxisController::update(); + + //get response of level PID + const auto& level_goal = goal_->getGoalValue(); + + TReal goal_angle = level_goal[axis_]; + TReal measured_angle = state_estimator_->getAngles()[axis_]; + + adjustToMinDistanceAngles(measured_angle, goal_angle); + + pid_->setGoal(goal_angle); + pid_->setMeasured(measured_angle); + pid_->update(); + + //use this to drive rate controller + rate_goal_[axis_] = pid_->getOutput() * params_->angle_rate_pid.max_limit[axis_]; + rate_controller_->update(); + + //rate controller's output is final output + output_ = rate_controller_->getOutput(); + } + + virtual TReal getOutput() override + { + return output_; + } + + /******************** IGoal ********************/ + virtual const Axis4r& getGoalValue() const override + { + return rate_goal_; + } + + virtual const GoalMode& getGoalMode() const override + { + return rate_mode_; + } + +private: + static void adjustToMinDistanceAngles(TReal& angle1, TReal& angle2) + { + static constexpr TReal TwoPi = 2 * M_PIf; + + //first make sure both angles are restricted from -360 to +360 + angle1 = static_cast(std::fmod(angle1, TwoPi)); + angle2 = static_cast(std::fmod(angle2, TwoPi)); + + //now make sure both angles are restricted from 0 to 360 + if (angle1 < 0) + angle1 = TwoPi + angle1; + if (angle2 < 0) + angle2 = TwoPi + angle2; + + //measure distance between two angles + auto dist = angle1 - angle2; + + //if its > 180 then invert first angle + if (dist > M_PIf) + angle1 = angle1 - TwoPi; + //if two much on other side then invert second angle + else if (dist < -M_PIf) + angle2 = angle2 - TwoPi; + } + +private: + unsigned int axis_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + + GoalMode rate_mode_; + Axis4r rate_goal_; + + TReal output_; + + Params* params_; + const IBoardClock* clock_; + std::unique_ptr> pid_; + std::unique_ptr rate_controller_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleRateController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleRateController.hpp new file mode 100644 index 000000000..04e0437b5 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/AngleRateController.hpp @@ -0,0 +1,74 @@ + +#pragma once + +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/IAxisController.hpp" +#include "Params.hpp" +#include "PidController.hpp" +#include "common/common_utils/Utils.hpp" +#include +#include +#include + +namespace Rov_simple +{ + +class AngleRateController : public IAxisController +{ +public: + AngleRateController(Params* params, const IBoardClock* clock) + : params_(params), clock_(clock) + { + } + + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + if (axis > 2) + throw std::invalid_argument("AngleRateController only supports axis 0-2 but it was " + std::to_string(axis)); + + axis_ = axis; + goal_ = goal; + state_estimator_ = state_estimator; + + pid_.reset(new PidController(clock_, + PidConfig(params_->angle_rate_pid.p[axis], params_->angle_rate_pid.i[axis], params_->angle_rate_pid.d[axis]))); + } + + virtual void reset() override + { + IAxisController::reset(); + + pid_->reset(); + output_ = TReal(); + } + + virtual void update() override + { + IAxisController::update(); + + pid_->setGoal(goal_->getGoalValue()[axis_]); + pid_->setMeasured(state_estimator_->getAngularVelocity()[axis_]); + pid_->update(); + + output_ = pid_->getOutput(); + } + + virtual TReal getOutput() override + { + return output_; + } + +private: + unsigned int axis_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + + TReal output_; + + Params* params_; + const IBoardClock* clock_; + std::unique_ptr> pid_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/CascadeController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/CascadeController.hpp new file mode 100644 index 000000000..605a60f94 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/CascadeController.hpp @@ -0,0 +1,147 @@ +#pragma once + +#include +#include +#include "interfaces/IController.hpp" +#include "interfaces/IStateEstimator.hpp" +#include "interfaces/ICommLink.hpp" +#include "interfaces/IGoal.hpp" +#include "AngleRateController.hpp" +#include "AngleLevelController.hpp" +#include "PassthroughController.hpp" +#include "ConstantOutputController.hpp" +#include "VelocityController.hpp" +#include "PositionController.hpp" +#include "common/common_utils/Utils.hpp" + +namespace Rov_simple +{ + +class CascadeController : public IController +{ +public: + CascadeController(Params* params, const IBoardClock* clock, ICommLink* comm_link) + : params_(params), clock_(clock), comm_link_(comm_link) + { + } + + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + goal_ = goal; + state_estimator_ = state_estimator; + } + + virtual void reset() override + { + IController::reset(); + + last_goal_mode_ = GoalMode::getUnknown(); + output_ = Axis4r(); + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (axis_controllers_[axis] != nullptr) + axis_controllers_[axis]->reset(); + } + } + + virtual void update() override + { + IController::update(); + + const auto& goal_mode = goal_->getGoalMode(); + const auto& goal_val = goal_->getGoalValue(); + + //common_utils::Utils::log(common_utils::Utils::stringf("Pos: %s", state_estimator_->getPosition().toString().c_str())); + + //if (!goal_mode.equals4(last_goal_mode_)) { + // common_utils::Utils::log(common_utils::Utils::stringf("GoalMode: %s", goal_mode.toString().c_str())); + //} + if (!goal_val.equals4(last_goal_val_)) { + //common_utils::Utils::log(common_utils::Utils::stringf("GoalVal : %s", goal_val.toString().c_str())); + last_goal_val_ = goal_val; + } + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + //re-create axis controllers if goal mode was changed since last time, or if gains have been updated + if (goal_mode[axis] != last_goal_mode_[axis] || params_->gains_changed == true) { + switch (goal_mode[axis]) { + case GoalModeType::AngleRate: + axis_controllers_[axis].reset(new AngleRateController(params_, clock_)); + break; + case GoalModeType::AngleLevel: + axis_controllers_[axis].reset(new AngleLevelController(params_, clock_)); + break; + case GoalModeType::VelocityWorld: + axis_controllers_[axis].reset(new VelocityController(params_, clock_)); + break; + case GoalModeType::PositionWorld: + axis_controllers_[axis].reset(new PositionController(params_, clock_)); + break; + case GoalModeType::Passthrough: + axis_controllers_[axis].reset(new PassthroughController()); + break; + case GoalModeType::Unknown: + axis_controllers_[axis].reset(nullptr); + break; + case GoalModeType::ConstantOutput: + axis_controllers_[axis].reset(new ConstantOutputController()); + break; + default: + throw std::invalid_argument("Axis controller type is not yet implemented for axis " + std::to_string(axis)); + } + last_goal_mode_[axis] = goal_mode[axis]; + + //initialize axis controller + if (axis_controllers_[axis] != nullptr) { + axis_controllers_[axis]->initialize(axis, goal_, state_estimator_); + axis_controllers_[axis]->reset(); + } + } + + //update axis controller + if (axis_controllers_[axis] != nullptr) { + axis_controllers_[axis]->update(); + output_[axis] = axis_controllers_[axis]->getOutput(); + } + else + comm_link_->log(std::string("Axis controller type is not set for axis ").append(std::to_string(axis)), ICommLink::kLogLevelInfo); + } + params_->gains_changed = false; + } + + virtual const Axis4r& getOutput() override + { + return output_; + } + + virtual bool isLastGoalModeAllPassthrough() override + { + is_last_goal_mode_all_passthrough_ = true; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + if (last_goal_mode_[axis] != GoalModeType::Passthrough) { + is_last_goal_mode_all_passthrough_ = false; + } + } + + return is_last_goal_mode_all_passthrough_; + } + +private: + Params* params_; + const IBoardClock* clock_; + + const IGoal* goal_; + const IStateEstimator* state_estimator_; + ICommLink* comm_link_; + + Axis4r output_; + + GoalMode last_goal_mode_; + Axis4r last_goal_val_; + bool is_last_goal_mode_all_passthrough_; + + std::unique_ptr axis_controllers_[Axis4r::AxisCount()]; +}; + +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/ConstantOutputController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/ConstantOutputController.hpp new file mode 100644 index 000000000..b7f896a41 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/ConstantOutputController.hpp @@ -0,0 +1,53 @@ + +#pragma once + +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/IAxisController.hpp" +#include "common/common_utils//Utils.hpp" +#include "Params.hpp" +#include + +namespace Rov_simple +{ + +class ConstantOutputController : public IAxisController +{ +public: + ConstantOutputController(TReal update_output = TReal(), TReal reset_output = TReal()) + : update_output_(update_output), reset_output_(reset_output) + { + } + + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + axis_ = axis; + unused(goal); + unused(state_estimator); + } + + virtual void reset() override + { + IAxisController::reset(); + output_ = reset_output_; + } + + virtual void update() override + { + IAxisController::update(); + output_ = update_output_; + } + + virtual TReal getOutput() override + { + return output_; + } + +private: + unsigned int axis_; + TReal update_output_; + TReal reset_output_; + TReal output_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/DoNothingController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/DoNothingController.hpp new file mode 100644 index 000000000..b14c0afac --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/DoNothingController.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "interfaces/IController.hpp" +#include "interfaces/IStateEstimator.hpp" +#include "interfaces/ICommLink.hpp" +#include "interfaces/IGoal.hpp" + +namespace Rov_simple +{ + +//This controller literally does nothing. It just implements the pure virtual IController functions. +//Use it if you need to use Passthrough commands but you have more than 4 actuator inputs, and +//call the commandMotorPWMs api function which overrides the actuator outputs in the firmware. +class DoNothingController : public IController +{ +public: + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + unused(goal); + unused(state_estimator); + + output_ = Axis4r(); + } + virtual const Axis4r& getOutput() override + { + return output_; + } + virtual bool isLastGoalModeAllPassthrough() override + { + return true; + } + +private: + Axis4r output_; +}; + +} //namespace \ No newline at end of file diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Firmware.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Firmware.hpp new file mode 100644 index 000000000..015e3e91a --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Firmware.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IBoard.hpp" +#include "interfaces/ICommLink.hpp" +#include "interfaces/IStateEstimator.hpp" +#include "interfaces/IFirmware.hpp" +#include "Params.hpp" +#include "RemoteControl.hpp" +#include "OffboardApi.hpp" +#include "Mixer.hpp" +#include "CascadeController.hpp" +// #include "AdaptiveController.hpp" +#include "DoNothingController.hpp" + +namespace Rov_simple +{ + +class Firmware : public IFirmware +{ +public: + Firmware(Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) + : params_(params), board_(board), comm_link_(comm_link), state_estimator_(state_estimator), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params), overridden_outputs_(false) + { + actuator_outputs_.assign(params_->actuator.actuator_count, 0.0f); + } + + virtual void reset() override + { + IFirmware::reset(); + + board_->reset(); + comm_link_->reset(); + // controller_->reset(); + offboard_api_.reset(); + + actuator_outputs_.assign(params_->actuator.actuator_count, 0.0f); + } + + virtual void update() override + { + IFirmware::update(); + + board_->update(); + offboard_api_.update(); + + // no-op since controller is set to DoNothingController + // controller_->update(); + // const Axis4r& output_controls = controller_->getOutput(); + + //write the actuator outputs + for (uint16_t actuator_index = 0; actuator_index < params_->actuator.actuator_count; ++actuator_index) { + float output = actuator_index < actuator_outputs_.size() ? actuator_outputs_[actuator_index] : 0.0f; + board_->writeOutput(actuator_index, output); + } + + comm_link_->update(); + } + + virtual IOffboardApi& offboardApi() override + { + return offboard_api_; + } + + virtual void overrideActuatorOutputs(const std::vector& values) override + { + actuator_outputs_ = values; + if (actuator_outputs_.size() < params_->actuator.actuator_count) { + actuator_outputs_.resize(params_->actuator.actuator_count, 0.0f); + } + } + +private: + //objects we use + Params* params_; + IBoard* board_; + ICommLink* comm_link_; + IStateEstimator* state_estimator_; + + OffboardApi offboard_api_; + Mixer mixer_; + // std::unique_ptr controller_; + + std::vector actuator_outputs_; + bool overridden_outputs_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Mixer.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Mixer.hpp new file mode 100644 index 000000000..6434755cc --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Mixer.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include "Params.hpp" +#include "interfaces/CommonStructs.hpp" + +namespace Rov_simple +{ + +class Mixer +{ +public: + Mixer(const Params* params) + : params_(params) + { + } + + void getMotorOutput(const Axis4r& controls, std::vector& motor_outputs) const + { + if (motor_outputs.size() < static_cast(kMotorCount)) { + motor_outputs.resize(kMotorCount, 0.0f); + } + + if (controls.throttle() < params_->actuator.min_angling_throttle) { + motor_outputs.assign(params_->actuator.actuator_count, controls.throttle()); + return; + } + + for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) { + motor_outputs[motor_index] = + controls.throttle() * mixerRov8[motor_index].throttle + controls.pitch() * mixerRov8[motor_index].pitch + controls.roll() * mixerRov8[motor_index].roll + controls.yaw() * mixerRov8[motor_index].yaw; + } + + float min_motor = *std::min_element(motor_outputs.begin(), motor_outputs.begin() + kMotorCount); + if (min_motor < params_->actuator.min_actuator_output) { + float undershoot = params_->actuator.min_actuator_output - min_motor; + for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) + motor_outputs[motor_index] += undershoot; + } + + float max_motor = *std::max_element(motor_outputs.begin(), motor_outputs.begin() + kMotorCount); + float scale = max_motor / params_->actuator.max_actuator_output; + if (scale > params_->actuator.max_actuator_output) { + for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) + motor_outputs[motor_index] /= scale; + } + + for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) + motor_outputs[motor_index] = std::max(params_->actuator.min_actuator_output, + std::min(motor_outputs[motor_index], params_->actuator.max_actuator_output)); + } + +private: + static const int kMotorCount = 8; + + const Params* params_; + + // Custom mixer data per motor + typedef struct motorMixer_t + { + float throttle; + float roll; + float pitch; + float yaw; + } motorMixer_t; + + // BlueROV2 Heavy 8-thruster configuration + // r1-r4: Vertical thrusters (heave/throttle, roll, pitch) + // r5-r8: Horizontal vectored thrusters (surge, sway, yaw) + const motorMixer_t mixerRov8[8] = { + // Vertical thrusters (r1-r4) + { 1.0f, -1.0f, 1.0f, 0.0f }, // r1: FRONT_R VERTICAL + { 1.0f, 1.0f, 1.0f, 0.0f }, // r2: FRONT_L VERTICAL + { 1.0f, 1.0f, -1.0f, 0.0f }, // r3: REAR_L VERTICAL + { 1.0f, -1.0f, -1.0f, 0.0f }, // r4: REAR_R VERTICAL + // Horizontal vectored thrusters (r5-r8) + { 0.0f, 0.0f, 0.0f, 1.0f }, // r5: FRONT_R HORIZONTAL + { 0.0f, 0.0f, 0.0f, -1.0f }, // r6: FRONT_L HORIZONTAL + { 0.0f, 0.0f, 0.0f, 1.0f }, // r7: REAR_L HORIZONTAL + { 0.0f, 0.0f, 0.0f, -1.0f }, // r8: REAR_R HORIZONTAL + }; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/OffboardApi.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/OffboardApi.hpp new file mode 100644 index 000000000..757662274 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/OffboardApi.hpp @@ -0,0 +1,275 @@ +#pragma once + +#include +#include "interfaces/ICommLink.hpp" +#include "interfaces/IGoal.hpp" +#include "interfaces/IOffboardApi.hpp" +#include "interfaces/IUpdatable.hpp" +#include "interfaces/CommonStructs.hpp" +#include "RemoteControl.hpp" +#include "Params.hpp" + +namespace Rov_simple +{ + +class OffboardApi : public IUpdatable + , public IOffboardApi +{ +public: + OffboardApi(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs, + IStateEstimator* state_estimator, ICommLink* comm_link) + : params_(params), rc_(params, clock, board_inputs, &vehicle_state_, state_estimator, comm_link), state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock), landed_(true) + { + } + + virtual void reset() override + { + IUpdatable::reset(); + + vehicle_state_.setState(params_->default_vehicle_state, state_estimator_->getGeoPoint()); + rc_.reset(); + has_api_control_ = false; + landed_ = true; + takenoff_ = false; + goal_timestamp_ = clock_->millis(); + updateGoalFromRc(); + } + + virtual void update() override + { + IUpdatable::update(); + + rc_.update(); + if (!has_api_control_) + updateGoalFromRc(); + else { + if (takenoff_ && + (clock_->millis() - goal_timestamp_ > params_->api_goal_timeout)) { + if (!is_api_timedout_) { + comm_link_->log("API call was not received, entering hover mode for safety"); + goal_mode_ = GoalMode::getPositionMode(); + goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true); + is_api_timedout_ = true; + } + + //do not update goal_timestamp_ + } + } + //else leave the goal set by IOffboardApi API + + detectLanding(); + detectTakingOff(); + } + + /**************** IOffboardApi ********************/ + + virtual const Axis4r& getGoalValue() const override + { + return goal_; + } + + virtual const GoalMode& getGoalMode() const override + { + return goal_mode_; + } + + virtual bool canRequestApiControl(std::string& message) override + { + if (rc_.allowApiControl()) + return true; + else { + message = "Remote Control switch position disallows API control"; + comm_link_->log(message, ICommLink::kLogLevelError); + return false; + } + } + virtual bool hasApiControl() override + { + return has_api_control_; + } + virtual bool requestApiControl(std::string& message) override + { + if (canRequestApiControl(message)) { + has_api_control_ = true; + + //initial value from RC for smooth transition + updateGoalFromRc(); + + comm_link_->log("requestApiControl was successful", ICommLink::kLogLevelInfo); + + return true; + } + else { + comm_link_->log("requestApiControl failed", ICommLink::kLogLevelError); + return false; + } + } + virtual void releaseApiControl() override + { + has_api_control_ = false; + comm_link_->log("releaseApiControl was successful", ICommLink::kLogLevelInfo); + } + virtual bool setGoalAndMode(const Axis4r* goal, const GoalMode* goal_mode, std::string& message) override + { + if (has_api_control_) { + if (goal != nullptr) + goal_ = *goal; + if (goal_mode != nullptr) + goal_mode_ = *goal_mode; + goal_timestamp_ = clock_->millis(); + is_api_timedout_ = false; + return true; + } + else { + message = "requestApiControl() must be called before using API control"; + comm_link_->log(message, ICommLink::kLogLevelError); + return false; + } + } + + virtual bool arm(std::string& message) override + { + if (has_api_control_) { + if (vehicle_state_.getState() == VehicleStateType::Armed) { + message = "Vehicle is already armed"; + comm_link_->log(message, ICommLink::kLogLevelInfo); + return true; + } + else if ((vehicle_state_.getState() == VehicleStateType::Inactive || vehicle_state_.getState() == VehicleStateType::Disarmed || vehicle_state_.getState() == VehicleStateType::BeingDisarmed)) { + + vehicle_state_.setState(VehicleStateType::Armed, state_estimator_->getHomeGeoPoint()); + goal_ = Axis4r(0, 0, 0, params_->rc.min_angling_throttle); + goal_mode_ = GoalMode::getAllRateMode(); + + message = "Vehicle is armed"; + comm_link_->log(message, ICommLink::kLogLevelInfo); + return true; + } + else { + message = "Vehicle cannot be armed because it is not in Inactive, Disarmed or BeingDisarmed state"; + comm_link_->log(message, ICommLink::kLogLevelError); + return false; + } + } + else { + message = "Vehicle cannot be armed via API because API has not been given control"; + comm_link_->log(message, ICommLink::kLogLevelError); + return false; + } + } + + virtual bool disarm(std::string& message) override + { + if (has_api_control_ && (vehicle_state_.getState() == VehicleStateType::Active || vehicle_state_.getState() == VehicleStateType::Armed || vehicle_state_.getState() == VehicleStateType::BeingArmed)) { + + vehicle_state_.setState(VehicleStateType::Disarmed); + goal_ = Axis4r(0, 0, 0, 0); + goal_mode_ = GoalMode::getAllRateMode(); + + message = "Vehicle is disarmed"; + comm_link_->log(message, ICommLink::kLogLevelInfo); + return true; + } + else { + message = "Vehicle cannot be disarmed because it is not in Active, Armed or BeingArmed state"; + comm_link_->log(message, ICommLink::kLogLevelError); + return false; + } + } + + virtual VehicleStateType getVehicleState() const override + { + return vehicle_state_.getState(); + } + + virtual const IStateEstimator& getStateEstimator() override + { + return *state_estimator_; + } + + virtual GeoPoint getHomeGeoPoint() const override + { + return state_estimator_->getHomeGeoPoint(); + } + + virtual GeoPoint getGeoPoint() const override + { + return state_estimator_->getGeoPoint(); + } + + virtual bool getLandedState() const override + { + return landed_; + } + +private: + void updateGoalFromRc() + { + goal_ = rc_.getGoalValue(); + goal_mode_ = rc_.getGoalMode(); + } + + void detectLanding() + { + + // if we are not trying to move by setting motor outputs + if (takenoff_) { + //if (!isGreaterThanArmedThrottle(goal_.throttle())) { + float checkThrottle = rc_.getMotorOutput(); + if (!isGreaterThanArmedThrottle(checkThrottle)) { + // and we are not currently moving (based on current velocities) + auto angular = state_estimator_->getAngularVelocity(); + auto velocity = state_estimator_->getLinearVelocity(); + if (isAlmostZero(angular.roll()) && isAlmostZero(angular.pitch()) && isAlmostZero(angular.yaw()) && + isAlmostZero(velocity.x()) && isAlmostZero(velocity.y()) && isAlmostZero(velocity.z())) { + // then we must be landed... + landed_ = true; + takenoff_ = false; + } + } + } + } + + void detectTakingOff() + { + // if we are not trying to move by setting motor outputs + if (!takenoff_) { + float checkThrottle = rc_.getMotorOutput(); + //TODO: better handling of landed & takenoff states + if (isGreaterThanArmedThrottle(checkThrottle) && + std::abs(state_estimator_->getLinearVelocity().z()) > 0.01f) { + takenoff_ = true; + landed_ = false; + } + } + } + + bool isAlmostZero(float v) + { + return std::abs(v) < kMovementTolerance; + } + bool isGreaterThanArmedThrottle(float throttle) + { + return throttle > params_->min_armed_throttle(); + } + +private: + const TReal kMovementTolerance = (TReal)0.08; + const Params* params_; + RemoteControl rc_; + IStateEstimator* state_estimator_; + ICommLink* comm_link_; + const IBoardClock* clock_; + + VehicleState vehicle_state_; + + Axis4r goal_; + GoalMode goal_mode_; + uint64_t goal_timestamp_; + + bool has_api_control_; + bool is_api_timedout_; + bool landed_, takenoff_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Params.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Params.hpp new file mode 100644 index 000000000..67a61ba13 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/Params.hpp @@ -0,0 +1,142 @@ +#pragma once + +#include "interfaces/CommonStructs.hpp" + +namespace Rov_simple +{ + +struct Params +{ +public: + static float min_armed_throttle() + { + static float val = 0.1f; + return val; + } + + //this should match up with target board + //simulation board should respect possible values + struct Actuator + { + uint16_t actuator_count = 8; // BlueROV2 Heavy 8 thrusters + float min_actuator_output = 0; + float max_actuator_output = 1; + //if min_armed_output too low then noise in pitch/roll can destabilize quad copter when throttle is zero + float min_angling_throttle = Params::min_armed_throttle() / 2; + } actuator; + + struct Rc + { + uint16_t channel_count = 12; + uint16_t read_interval_ms = 10; + int16_t rate_level_mode_channel = 4; //corresponds to switch 0 in rc_data + int16_t allow_api_control_channel = 5; //corresponds to switch 1 in rc_data + + //When actions such as arming/unarming, how much tolerance can be allowed in stick positions from 0 to 1? + float action_request_tolerance = 0.1f; + + //milliseconds while sticks should stay in position + uint64_t arm_duration = 100; + uint64_t disarm_duration = 100; + uint64_t neutral_duration = 100; + + Axis4 channels = Axis4(0, 3, 1, 2); + + TReal max_angle_level_switch = 0.3f; + + //should be >= motor.min_angling_throttle + float min_angling_throttle = Params::min_armed_throttle() / 1.5f; + + bool allow_api_when_disconnected = false; + bool allow_api_always = false; + + } rc; + + struct AngleRatePid + { + //max_xxx_rate > 5 would introduce wobble/oscillations + const float kMaxLimit = 2.5f; + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + + Axis3r max_limit = Axis3r(kMaxLimit, kMaxLimit, kMaxLimit); //roll, pitch, yaw - in radians/sec + + //p_xxx_rate params are sensitive to gyro noise. Values higher than 0.5 would require + //noise filtration + Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); + } angle_rate_pid; + + struct AngleLevelPid + { + const float pi = 3.14159265359f; //180-degrees + const float kP = 2.5f; + const float kI = 0.0f; + const float kD = 0.0f; + + //max_pitch/roll_angle > 5.5 would produce verticle thrust that is not enough to keep vehicle in air at extremities of controls + Axis4r max_limit = Axis4r(pi / 5.5f, pi / 5.5f, pi, 1.0f); //roll, pitch, yaw - in radians/sec + + Axis4r p = Axis4r(kP, kP, kP, 1.0f); + Axis4r i = Axis4r(kI, kI, kI, 0.0f); + Axis4r d = Axis4r(kD, kD, kD, 0.0f); + } angle_level_pid; + + struct PositionPid + { + const float kMaxLimit = 8.8E26f; //some big number like size of known universe + const float kP = 0.25f; + const float kI = 0.0f; + const float kD = 0.0f; + + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, kMaxLimit, 1.0f); //x, y, z in meters + + Axis4r p = Axis4r(kP, kP, 0, kP); + Axis4r i = Axis4r(kI, kI, kI, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); + } position_pid; + + struct VelocityPid + { + const float kMinThrottle = std::min(1.0f, Params::min_armed_throttle() * 3.0f); + const float kMaxLimit = 6.0f; // m/s + const float kP = 0.2f; + const float kI = 2.0f; + const float kD = 0.0f; + + Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, 0, kMaxLimit); //x, y, yaw, z in meters + + Axis4r p = Axis4r(kP, kP, 0.0f, 2.0f); // todo why 2.0f hardcoded + Axis4r i = Axis4r(0.0f, 0.0f, 0.0f, kI); + Axis4r d = Axis4r(kD, kD, kD, kD); + + Axis4r iterm_discount = Axis4r(1, 1, 1, 0.9999f); + Axis4r output_bias = Axis4r(0, 0, 0, 0); + + //we keep min throttle higher so that if we are angling a lot, its still supported + float min_throttle = kMinThrottle; + } velocity_pid; + + struct Takeoff + { + float takeoff_z = -2.0f; + //float velocity = -1.0f; + } takeoff; + + enum class ControllerType + { + Cascade, + Adaptive, + DoNothing //use this control type if output values will be entirely overridden every time + }; + + GoalMode default_goal_mode = GoalMode::getStandardAngleMode(); + VehicleStateType default_vehicle_state = VehicleStateType::Inactive; + uint64_t api_goal_timeout = 60; //milliseconds + ControllerType controller_type = ControllerType::DoNothing; + bool gains_changed; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PassthroughController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PassthroughController.hpp new file mode 100644 index 000000000..e1e2a5d77 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PassthroughController.hpp @@ -0,0 +1,47 @@ + +#pragma once + +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/IAxisController.hpp" +#include "common/common_utils//Utils.hpp" +#include "Params.hpp" +#include + +namespace Rov_simple +{ + +class PassthroughController : public IAxisController +{ +public: + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + axis_ = axis; + goal_ = goal; + unused(state_estimator); + } + + virtual void reset() override + { + IAxisController::reset(); + output_ = TReal(); + } + + virtual void update() override + { + IAxisController::update(); + output_ = goal_->getGoalValue()[axis_]; + } + + virtual TReal getOutput() override + { + return output_; + } + +private: + unsigned int axis_; + const IGoal* goal_; + TReal output_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PidController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PidController.hpp new file mode 100644 index 000000000..c6ea74062 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PidController.hpp @@ -0,0 +1,133 @@ +#pragma once + +#include +#include +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IPidIntegrator.hpp" +#include "StdPidIntegrator.hpp" +#include "RungKuttaPidIntegrator.hpp" + +namespace Rov_simple +{ + +template +class PidController : public IUpdatable +{ +public: + PidController(const IBoardClock* clock = nullptr, const PidConfig& config = PidConfig()) + : clock_(clock), config_(config) + { + switch (config.integrator_type) { + case PidConfig::IntegratorType::Standard: + integrator = std::unique_ptr>(new StdPidIntegrator(config_)); + break; + case PidConfig::IntegratorType::RungKutta: + integrator = std::unique_ptr>(new RungKuttaPidIntegrator(config_)); + break; + default: + throw std::invalid_argument("PID integrator type is not recognized"); + } + } + + void setGoal(const T& goal) + { + goal_ = goal; + } + const T& getGoal() const + { + return goal_; + } + + void setMeasured(const T& measured) + { + measured_ = measured; + } + const T& getMeasured() const + { + return measured_; + } + + const PidConfig& getConfig() const + { + return config_; + } + + //allow changing config at runtime + void setConfig(const PidConfig& config) + { + bool renabled = !config_.enabled && config.enabled; + config_ = config; + + if (renabled) { + last_error_ = goal_ - measured_; + integrator->set(output_); + } + } + + T getOutput() + { + return output_; + } + + virtual void reset() override + { + IUpdatable::reset(); + + goal_ = T(); + measured_ = T(); + last_time_ = clock_ == nullptr ? 0 : clock_->millis(); + integrator->reset(); + last_error_ = goal_ - measured_; + min_dt_ = config_.time_scale * config_.time_scale; + } + + virtual void update() override + { + IUpdatable::update(); + + if (!config_.enabled) + return; + + const T error = goal_ - measured_; + + float dt = clock_ == nullptr ? 1 : (clock_->millis() - last_time_) * config_.time_scale; + + float pterm = error * config_.kp; + float dterm = 0; + if (dt > min_dt_) { + integrator->update(dt, error, last_time_); + + float error_der = dt > 0 ? (error - last_error_) / dt : 0; + dterm = error_der * config_.kd; + last_error_ = error; + } + + output_ = config_.output_bias + pterm + integrator->getOutput() + dterm; + + //limit final output + output_ = clip(output_, config_.min_output, config_.max_output); + + last_time_ = clock_->millis(); + } + +private: + //TODO: replace with std::clamp after moving to C++17 + static T clip(T val, T min_value, T max_value) + { + return std::max(min_value, std::min(val, max_value)); + } + +private: + T goal_, measured_; + T output_; + uint64_t last_time_; + const IBoardClock* clock_; + + float last_error_; + float min_dt_; + const PidConfig config_; + + std::unique_ptr> integrator; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PositionController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PositionController.hpp new file mode 100644 index 000000000..ab46a5bbe --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/PositionController.hpp @@ -0,0 +1,110 @@ + +#pragma once + +#include "interfaces/IUpdatable.hpp" +#include "interfaces/IGoal.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IAxisController.hpp" +#include "VelocityController.hpp" +#include "Params.hpp" +#include "PidController.hpp" +#include "common/common_utils/Utils.hpp" + +namespace Rov_simple +{ + +class PositionController : public IAxisController + , + public IGoal //for internal child controller +{ +public: + PositionController(Params* params, const IBoardClock* clock = nullptr) + : params_(params), clock_(clock) + { + } + + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + if (axis == 2) + throw std::invalid_argument("PositionController does not support yaw axis i.e. " + std::to_string(axis)); + + axis_ = axis; + goal_ = goal; + state_estimator_ = state_estimator; + + //initialize parent PID + pid_.reset(new PidController(clock_, + PidConfig(params_->position_pid.p[axis], params_->position_pid.i[axis], params_->position_pid.d[axis]))); + + //initialize child controller + velocity_controller_.reset(new VelocityController(params_, clock_)); + velocity_controller_->initialize(axis, this, state_estimator_); + + //we will be setting goal for child controller so we need these two things + velocity_mode_ = GoalMode::getUnknown(); + velocity_mode_[axis] = GoalModeType::VelocityWorld; + } + + virtual void reset() override + { + IAxisController::reset(); + + pid_->reset(); + velocity_controller_->reset(); + velocity_goal_ = Axis4r(); + output_ = TReal(); + } + + virtual void update() override + { + IAxisController::update(); + + const Axis4r& goal_position_world = goal_->getGoalValue(); + pid_->setGoal(goal_position_world[axis_]); + const Axis4r& measured_position_world = Axis4r::xyzToAxis4( + state_estimator_->getPosition(), true); + pid_->setMeasured(measured_position_world[axis_]); + pid_->update(); + + //use this to drive child controller + velocity_goal_[axis_] = pid_->getOutput() * params_->velocity_pid.max_limit[axis_]; + velocity_controller_->update(); + + //final output + output_ = velocity_controller_->getOutput(); + } + + virtual TReal getOutput() override + { + return output_; + } + + /******************** IGoal ********************/ + virtual const Axis4r& getGoalValue() const override + { + return velocity_goal_; + } + + virtual const GoalMode& getGoalMode() const override + { + return velocity_mode_; + } + +private: + unsigned int axis_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + + GoalMode velocity_mode_; + Axis4r velocity_goal_; + + TReal output_; + + Params* params_; + const IBoardClock* clock_; + std::unique_ptr> pid_; + std::unique_ptr velocity_controller_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RemoteControl.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RemoteControl.hpp new file mode 100644 index 000000000..66bd86d55 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RemoteControl.hpp @@ -0,0 +1,267 @@ +#pragma once + +#include +#include +#include "interfaces/IBoardClock.hpp" +#include "interfaces/IBoardInputPins.hpp" +#include "interfaces/IGoal.hpp" +#include "interfaces/CommonStructs.hpp" + +namespace Rov_simple +{ + +class RemoteControl : public IGoal + , public IUpdatable +{ +public: + RemoteControl(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs, + VehicleState* vehicle_state, IStateEstimator* state_estimator, ICommLink* comm_link) + : params_(params), clock_(clock), board_inputs_(board_inputs), vehicle_state_(vehicle_state), state_estimator_(state_estimator), comm_link_(comm_link) + { + } + + virtual void reset() override + { + IUpdatable::reset(); + + goal_ = Axis4r::zero(); + goal_mode_ = params_->default_goal_mode; + allow_api_control_ = params_->rc.allow_api_always; + last_rec_read_ = 0; + last_angle_mode_ = std::numeric_limits::min(); + request_duration_ = 0; + } + + virtual void update() override + { + IUpdatable::update(); + + uint64_t time = clock_->millis(); + + //don't keep reading if not updated + uint64_t dt = time - last_rec_read_; + if (dt <= params_->rc.read_interval_ms) + return; + last_rec_read_ = time; + + //read channel values + Axis4r channels; + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) { + channels[axis] = board_inputs_->isRcConnected() ? board_inputs_->readChannel(params_->rc.channels[axis]) + : 0; + } + + //set goal mode as per the switch position on RC + updateGoalMode(); + updateAllowApiControl(); + + //get any special action being requested by user such as arm/disarm + RcRequestType rc_action = getActionRequest(channels); + + //state machine + switch (vehicle_state_->getState()) { + case VehicleStateType::Inactive: + //comm_link_->log(std::string("State:\t ").append("Inactive state")); + + if (rc_action == RcRequestType::ArmRequest) { + comm_link_->log(std::string("State:\t ").append("Inactive state, Arm request received")); + request_duration_ += dt; + + if (request_duration_ > params_->rc.arm_duration) { + vehicle_state_->setState(VehicleStateType::BeingArmed); + request_duration_ = 0; + } + } + //else ignore + break; + case VehicleStateType::BeingArmed: + comm_link_->log(std::string("State:\t ").append("Being armed")); + + //start the motors + goal_ = Axis4r::zero(); //neural activation while still being armed + goal_.throttle() = params_->Params::min_armed_throttle(); + + //we must wait until sticks are at neutral or we will have random behavior + if (rc_action == RcRequestType::NeutralRequest) { + request_duration_ += dt; + + if (request_duration_ > params_->rc.neutral_duration) { + //TODO: this code should be reused in OffboardApi + vehicle_state_->setState(VehicleStateType::Armed, state_estimator_->getHomeGeoPoint()); + comm_link_->log(std::string("State:\t ").append("Armed")); + request_duration_ = 0; + } + } + //else ignore + break; + case VehicleStateType::Armed: + //unless disarm is being requested, set goal from stick position + if (rc_action == RcRequestType::DisarmRequest) { + comm_link_->log(std::string("State:\t ").append("Armed state, disarm request received")); + request_duration_ += dt; + + if (request_duration_ > params_->rc.disarm_duration) { + vehicle_state_->setState(VehicleStateType::BeingDisarmed); + request_duration_ = 0; + } + } + else { + request_duration_ = 0; //if there was spurious disarm request + updateGoal(channels); + } + break; + case VehicleStateType::BeingDisarmed: + comm_link_->log(std::string("State:\t ").append("Being state")); + + //TODO: this code should be reused in OffboardApi + goal_.setAxis3(Axis3r::zero()); //neutral activation while being disarmed + vehicle_state_->setState(VehicleStateType::Disarmed); + request_duration_ = 0; + + break; + case VehicleStateType::Disarmed: + comm_link_->log(std::string("State:\t ").append("Disarmed")); + + goal_ = Axis4r::zero(); //neutral activation while being disarmed + vehicle_state_->setState(VehicleStateType::Inactive); + request_duration_ = 0; + + break; + default: + throw std::runtime_error("VehicleStateType has unknown value for RemoteControl::update()"); + } + } + + virtual const Axis4r& getGoalValue() const override + { + return goal_; + } + + virtual const GoalMode& getGoalMode() const override + { + return goal_mode_; + } + + bool allowApiControl() + { + return allow_api_control_; + } + + float getMotorOutput() + { + return board_inputs_->getAvgMotorOutput(); + } + +private: + enum class RcRequestType + { + None, + ArmRequest, + DisarmRequest, + NeutralRequest + }; + + void updateGoalMode() + { + if (!board_inputs_->isRcConnected()) { + //TODO: is it good idea to keep the last mode? + //if (!goal_mode_.equals4(GoalMode::getUnknown())) + // goal_mode_ = GoalMode::getUnknown(); + + //For angle as well as rate mode, keep only throttle + goal_.setAxis3(Axis3r()); + + return; + } + + //set up RC mode as level or rate + angle_mode_ = board_inputs_->readChannel(params_->rc.rate_level_mode_channel); + if (last_angle_mode_ != angle_mode_) { + //for 3 way switch, 1/3 value for each position + if (angle_mode_ < params_->rc.max_angle_level_switch) + goal_mode_ = GoalMode::getStandardAngleMode(); + else + goal_mode_ = GoalMode::getAllRateMode(); + + last_angle_mode_ = angle_mode_; + } + } + + void updateAllowApiControl() + { + bool allow = params_->rc.allow_api_always; + allow |= board_inputs_->isRcConnected() ? board_inputs_->readChannel(params_->rc.allow_api_control_channel) > 0.1f + : params_->rc.allow_api_when_disconnected; + + if (allow_api_control_ != allow) + comm_link_->log(std::string("API control enabled:\t").append(std::to_string(allow_api_control_))); + + allow_api_control_ = allow; + } + + void updateGoal(const Axis4r& channels) + { + //for 3 way switch, 1/3 value for each position + if (angle_mode_ < params_->rc.max_angle_level_switch) { + //we are in control-by-level mode + goal_ = channels.colWiseMultiply4(params_->angle_level_pid.max_limit); + } + else { //we are in control-by-rate mode + goal_ = channels.colWiseMultiply4(params_->angle_level_pid.max_limit); + } + + //if throttle is too low then set all motors to same value as throttle because + //otherwise values in pitch/roll/yaw would get clipped randomly and can produce random results + //in other words: we can't do angling if throttle is too low + if (channels.throttle() < params_->rc.min_angling_throttle) + goal_.throttle() = params_->rc.min_angling_throttle; + } + + static bool isInTolerance(TReal val, TReal tolerance, TReal center = TReal()) + { + return val <= center + tolerance && val >= center - tolerance; + } + + RcRequestType getActionRequest(const Axis4r& channels) + { + TReal tolerance = params_->rc.action_request_tolerance; + TReal stick_min = 1 - tolerance; + + bool yaw_action_positive = channels.yaw() >= stick_min; + bool yaw_action_negative = channels.yaw() <= -stick_min; + bool throttle_action = channels.throttle() <= tolerance; + + bool roll_action_positive = channels.roll() >= stick_min; + bool roll_action_negative = channels.roll() <= -stick_min; + TReal normalized_pitch = (channels.pitch() + 1) / 2; //-1 to 1 --> 0 to 1 + bool pitch_action = normalized_pitch >= stick_min; + + if (yaw_action_positive && throttle_action && roll_action_negative && pitch_action) + return RcRequestType::ArmRequest; + else if (yaw_action_negative && throttle_action && roll_action_positive && pitch_action) + return RcRequestType::DisarmRequest; + else if (isInTolerance(channels.roll(), tolerance) && isInTolerance(channels.pitch(), tolerance) && isInTolerance(channels.yaw(), tolerance)) + return RcRequestType::NeutralRequest; + else + return RcRequestType::None; + } + +private: + const Params* params_; + const IBoardClock* clock_; + const IBoardInputPins* board_inputs_; + VehicleState* vehicle_state_; + IStateEstimator* state_estimator_; + ICommLink* comm_link_; + + Axis4r goal_; + GoalMode goal_mode_; + + uint64_t last_rec_read_; + TReal angle_mode_, last_angle_mode_; + bool allow_api_control_; + + uint64_t request_duration_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RungKuttaPidIntegrator.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RungKuttaPidIntegrator.hpp new file mode 100644 index 000000000..4d606299a --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/RungKuttaPidIntegrator.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include "interfaces/CommonStructs.hpp" + +namespace Rov_simple +{ + +template +class RungKuttaPidIntegrator : public IPidIntegrator +{ +public: + RungKuttaPidIntegrator(const PidConfig& config) + : config_(config) + { + } + + virtual ~RungKuttaPidIntegrator() {} + + virtual void reset() override + { + iterm_int_ = T(); + } + + virtual void set(T val) override + { + iterm_int_ = val; + clipIterm(); + } + + virtual void update(float dt, T error, uint64_t last_time) override + { + unused(dt); + + error_int = error; + y[0] = error_int; + + rungeKutta(y, yp, static_cast(last_time), 0.003f, length); + + //don't let iterm grow beyond limits (integral windup) + clipIterm(); + } + + virtual T getOutput() override + { + return config_.ki * yp[0]; + } + +private: + void clipIterm() + { + yp[0] = clip(yp[0], config_.min_output, config_.max_output); + } + + //TODO: replace with std::clamp after moving to C++17 + static T clip(T val, T min_value, T max_value) + { + return std::max(min_value, std::min(val, max_value)); + } + + void model(float* y_val, float last_time_, float* y_out) + { + unused(last_time_); + y_out[0] = y_val[0]; + } + + void rungeKutta(float* y_val, float* yp_val, float time, float dt, int size) + { + float k1, k2, k3, k4; + float zero_vec[length] = { 0 }; + float* y_temp; + float* y_out = zero_vec; + for (int n = 0; n < size; n++) { + y_temp = y_val; + model(y_temp, time, y_out); + k1 = dt * y_out[n]; + + y_temp[n] = y_val[n] + k1 / 2; + model(y_temp, time + dt / 2, y_out); + k2 = dt * y_out[n]; + + y_temp[n] = y_val[n] + k2 / 2; + model(y_temp, time + dt / 2, y_out); + k3 = dt * y_out[n]; + + y_temp[n] = y_val[n] + k3; + model(y_temp, time + dt, y_out); + k4 = dt * y_out[n]; + + yp_val[n] = y_val[n] + k1 / 6 + k2 / 3 + k3 / 4 + k4 / 6; + } + } + +private: + float iterm_int_; + const PidConfig config_; + + static constexpr int length = 1; + float y_vec[length] = {}; + float* y = y_vec; + float yp_vec[length] = {}; + float* yp = yp_vec; + float error_int = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/StdPidIntegrator.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/StdPidIntegrator.hpp new file mode 100644 index 000000000..6bd97e4b7 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/StdPidIntegrator.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include "interfaces/CommonStructs.hpp" + +namespace Rov_simple +{ + +template +class StdPidIntegrator : public IPidIntegrator +{ +public: + StdPidIntegrator(const PidConfig& config) + : config_(config) + { + } + + virtual ~StdPidIntegrator() {} + + virtual void reset() override + { + iterm_int_ = T(); + } + + virtual void set(T val) override + { + iterm_int_ = val; + clipIterm(); + } + + virtual void update(float dt, T error, uint64_t last_time) override + { + unused(last_time); + + //to supoort changes in ki at runtime, we accumulate iterm + //instead of error + iterm_int_ = iterm_int_ * config_.iterm_discount + dt * error * config_.ki; + + //don't let iterm grow beyond limits (integral windup) + clipIterm(); + } + + virtual T getOutput() override + { + return iterm_int_; + } + +private: + void clipIterm() + { + iterm_int_ = clip(iterm_int_, config_.min_output, config_.max_output); + } + + //TODO: replace with std::clamp after moving to C++17 + static T clip(T val, T min_value, T max_value) + { + return std::max(min_value, std::min(val, max_value)); + } + +private: + float iterm_int_; + const PidConfig config_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/VelocityController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/VelocityController.hpp new file mode 100644 index 000000000..8583e7e27 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/VelocityController.hpp @@ -0,0 +1,156 @@ + +#pragma once + +#include "interfaces/IUpdatable.hpp" +#include "interfaces/IGoal.hpp" +#include "interfaces/IBoardClock.hpp" +#include "interfaces/CommonStructs.hpp" +#include "interfaces/IAxisController.hpp" +#include "AngleLevelController.hpp" +#include "Params.hpp" +#include "PidController.hpp" +#include "common/common_utils/Utils.hpp" + +namespace Rov_simple +{ + +class VelocityController : public IAxisController + , + public IGoal //for internal child controller +{ +public: + VelocityController(Params* params, const IBoardClock* clock = nullptr) + : params_(params), clock_(clock) + { + } + + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override + { + axis_ = axis; + goal_ = goal; + state_estimator_ = state_estimator; + + PidConfig pid_config(params_->velocity_pid.p[axis], + params_->velocity_pid.i[axis], + params_->velocity_pid.d[axis]); + pid_config.iterm_discount = params_->velocity_pid.iterm_discount[axis]; + pid_config.output_bias = params_->velocity_pid.output_bias[axis]; + + pid_.reset(new PidController(clock_, pid_config)); + + //we will be setting goal for child controller so we need these two things + child_mode_ = GoalMode::getUnknown(); + switch (axis_) { + case 0: + child_controller_.reset(new AngleLevelController(params_, clock_)); + child_mode_[axis_] = GoalModeType::AngleLevel; //vy = roll + break; + case 1: + child_controller_.reset(new AngleLevelController(params_, clock_)); + child_mode_[axis_] = GoalModeType::AngleLevel; //vx = - pitch + break; + case 2: + //we control yaw + throw std::invalid_argument("axis must be 0, 1 or 3 but it was " + std::to_string(axis_) + " because yaw cannot be controlled by VelocityController"); + case 3: + //not really required + //output of parent controller is -1 to 1 which + //we will transform to 0 to 1 + child_controller_.reset(new PassthroughController()); + child_mode_[axis_] = GoalModeType::Passthrough; + break; + default: + throw std::invalid_argument("axis must be 0 to 2"); + } + + //initialize child controller + child_controller_->initialize(axis_, this, state_estimator_); + } + + virtual void reset() override + { + IAxisController::reset(); + + pid_->reset(); + child_controller_->reset(); + child_goal_ = Axis4r(); + output_ = TReal(); + } + + virtual void update() override + { + IAxisController::update(); + + //First get PID output + const Axis3r& goal_velocity_world = Axis4r::axis4ToXyz( + goal_->getGoalValue(), true); + const Axis4r& goal_velocity_local = Axis4r::xyzToAxis4( + state_estimator_->transformToBodyFrame(goal_velocity_world), true); + pid_->setGoal(goal_velocity_local[axis_]); + + const Axis3r& measured_velocity_world = state_estimator_->getLinearVelocity(); + const Axis4r& measured_velocity_local = Axis4r::xyzToAxis4( + state_estimator_->transformToBodyFrame(measured_velocity_world), true); + pid_->setMeasured(measured_velocity_local[axis_]); + pid_->update(); + + //use this to drive child controller + switch (axis_) { + case 0: //+vy is +ve roll + child_goal_[axis_] = pid_->getOutput() * params_->angle_level_pid.max_limit[axis_]; + child_controller_->update(); + output_ = child_controller_->getOutput(); + + //if (std::abs(goal_velocity_local[axis_] - measured_velocity_local[axis_]) > 1) + // msr::airlib::Utils::log(msr::airlib::Utils::stringf("VC: %i\t%f\t%f\t%f", + // axis_, goal_velocity_local[axis_], + // measured_velocity_local[axis_], output_)); + + break; + case 1: //+vx is -ve pitch + child_goal_[axis_] = -pid_->getOutput() * params_->angle_level_pid.max_limit[axis_]; + child_controller_->update(); + output_ = child_controller_->getOutput(); + break; + case 3: //+vz is -ve throttle (NED coordinates) + output_ = (-pid_->getOutput() + 1) / 2; //-1 to 1 --> 0 to 1 + output_ = std::max(output_, params_->velocity_pid.min_throttle); + break; + default: + throw std::invalid_argument("axis must be 0, 1 or 3 for VelocityController"); + } + } + + virtual TReal getOutput() override + { + return output_; + } + + /******************** IGoal ********************/ + virtual const Axis4r& getGoalValue() const override + { + return child_goal_; + } + + virtual const GoalMode& getGoalMode() const override + { + return child_mode_; + } + +private: + unsigned int axis_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + + GoalMode child_mode_; + Axis4r child_goal_; + + TReal output_; + + Params* params_; + const IBoardClock* clock_; + std::unique_ptr> pid_; + std::unique_ptr child_controller_; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/CommonStructs.hpp new file mode 100644 index 000000000..98fc2ed9f --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/CommonStructs.hpp @@ -0,0 +1,373 @@ +#pragma once + +#include +#include + +namespace Rov_simple +{ + +typedef float TReal; + +template +class Axis3 +{ +public: + Axis3(const T& x_val = T(), const T& y_val = T(), const T& z_val = T()) + : vals_{ x_val, y_val, z_val } + { + } + + virtual ~Axis3() = default; + + //access by index + virtual T& operator[](unsigned int index) + { + return vals_[index]; + } + + virtual const T& operator[](unsigned int index) const + { + return vals_[index]; + } + + virtual std::string toString() const + { + return std::to_string(static_cast(vals_[0])) + .append(", ") + .append(std::to_string(static_cast(vals_[1]))) + .append(", ") + .append(std::to_string(static_cast(vals_[2]))); + } + + bool equals3(const Axis3& other) const + { + return vals_[0] == other.vals_[0] && vals_[1] == other.vals_[1] && vals_[2] == other.vals_[2]; + } + + Axis3 colWiseMultiply3(const Axis3& other) const + { + return Axis3(vals_[0] * other.vals_[0], vals_[1] * other.vals_[1], vals_[2] * other.vals_[2]); + } + + //access as axis + const T& x() const { return vals_[0]; } + const T& y() const { return vals_[1]; } + const T& z() const { return vals_[2]; } + T& x() { return vals_[0]; } + T& y() { return vals_[1]; } + T& z() { return vals_[2]; } + + //access as angles + const T& roll() const { return vals_[0]; } + const T& pitch() const { return vals_[1]; } + const T& yaw() const { return vals_[2]; } + T& roll() { return vals_[0]; } + T& pitch() { return vals_[1]; } + T& yaw() { return vals_[2]; } + + static const Axis3& zero() + { + static const Axis3 zero_val = Axis3(); + return zero_val; + } + + static constexpr unsigned int AxisCount() + { + return 3; + } + +private: + T vals_[3]; +}; +typedef Axis3 Axis3r; + +template +class Axis4 : public Axis3 +{ +public: + Axis4(const T& x_val = T(), const T& y_val = T(), const T& z_val = T(), const T& val4_val = T()) + : Axis3(x_val, y_val, z_val), val4_(val4_val) + { + } + + Axis4(const Axis3& axis3_val, const T& val4_val = T()) + : Axis3(axis3_val), val4_(val4_val) + { + } + + void setAxis3(const Axis3& axis3) + { + for (unsigned int axis = 0; axis < Axis3::AxisCount(); ++axis) + (*this)[axis] = axis3[axis]; + } + + void setValues(const std::vector& vals) + { + (*this)[0] = vals[0]; + (*this)[1] = vals[1]; + (*this)[2] = vals[2]; + val4_ = vals[3]; + } + + T& val4() + { + return val4_; + } + const T& val4() const + { + return val4_; + } + + //access by index + virtual T& operator[](unsigned int index) override + { + return const_cast( + (*const_cast*>(this))[index]); + } + virtual const T& operator[](unsigned int index) const override + { + if (index <= 2) + return Axis3::operator[](index); + else if (index == 3) + return val4_; + else + throw std::out_of_range("index must be <= 3 but it was " + std::to_string(index)); + } + virtual std::string toString() const override + { + return Axis3::toString() + .append(", ") + .append(std::to_string(static_cast(val4_))); + } + + bool equals4(const Axis4& other) const + { + return (*this)[0] == other[0] && (*this)[1] == other[1] && + (*this)[2] == other[2] && (*this)[3] == other[3]; + } + + Axis4 colWiseMultiply4(const Axis4& other) const + { + return Axis4((*this)[0] * other[0], (*this)[1] * other[1], (*this)[2] * other[2], (*this)[3] * other[3]); + } + + T& throttle() + { + return val4_; + } + const T& throttle() const + { + return val4_; + } + + static const Axis4& zero() + { + static const Axis4 zero_val = Axis4(); + return zero_val; + } + + static constexpr unsigned int AxisCount() + { + return 4; + } + + static Axis3 axis4ToXyz(const Axis4 axis4, bool swap_xy) + { + return Axis3(axis4[swap_xy ? 1 : 0], axis4[swap_xy ? 0 : 1], axis4[3]); + } + static Axis4 xyzToAxis4(const Axis3 xyz, bool swap_xy) + { + //TODO: use nan instead 0? + return Axis4(xyz[swap_xy ? 1 : 0], xyz[swap_xy ? 0 : 1], 0, xyz[2]); + } + +private: + T val4_ = 0; +}; +typedef Axis4 Axis4r; + +struct GeoPoint +{ + double latitude = std::numeric_limits::quiet_NaN(); + double longitude = std::numeric_limits::quiet_NaN(); + float altiude = std::numeric_limits::quiet_NaN(); + + GeoPoint() {} + + static const GeoPoint& nan() + { + const static GeoPoint val; + return val; + } +}; + +struct KinematicsState +{ + Axis3r position; + Axis4r orientation; + + Axis3r linear_velocity; + Axis3r angular_velocity; + + Axis3r linear_acceleration; + Axis3r angular_acceleration; +}; + +enum class VehicleStateType +{ + Unknown, + Inactive, + BeingArmed, + Armed, + Active, + BeingDisarmed, + Disarmed +}; + +class VehicleState +{ +public: + VehicleStateType getState() const + { + return state_; + } + void setState(VehicleStateType state, const GeoPoint& home_geo_point = GeoPoint::nan()) + { + if (state == VehicleStateType::Armed && std::isnan(home_geo_point.latitude)) + throw std::invalid_argument("home_geo_point must be supplied to set armed state"); + + state_ = state; + } + + static VehicleStateType fromString(const std::string& val) + { + if (val == "Armed") + return VehicleStateType::Armed; + if (val == "Inactive") + return VehicleStateType::Inactive; + if (val == "Unknown") + return VehicleStateType::Unknown; + if (val == "BeingArmed") + return VehicleStateType::BeingArmed; + if (val == "Active") + return VehicleStateType::Active; + if (val == "BeingDisarmed") + return VehicleStateType::BeingDisarmed; + if (val == "Disarmed") + return VehicleStateType::Disarmed; + + throw std::invalid_argument(std::string( + "The value cannot be converted to VehicleStateType enum: ") + .append(val)); + } + + const GeoPoint& getHomeGeoPoint() const + { + return home_geo_point_; + } + +private: + VehicleStateType state_ = VehicleStateType::Unknown; + GeoPoint home_geo_point_; +}; + +enum class GoalModeType : int +{ + Unknown = 0, + Passthrough, + AngleLevel, + AngleRate, + VelocityWorld, + PositionWorld, + ConstantOutput +}; + +class GoalMode : public Axis4 +{ +public: + GoalMode(GoalModeType x_val = GoalModeType::AngleLevel, GoalModeType y_val = GoalModeType::AngleLevel, + GoalModeType z_val = GoalModeType::AngleRate, GoalModeType val4_val = GoalModeType::Passthrough) + : Axis4(x_val, y_val, z_val, val4_val) + { + } + + static const GoalMode& getStandardAngleMode() + { + static const GoalMode mode = GoalMode(); + return mode; + } + + static const GoalMode& getVelocityXYPosZMode() + { + static const GoalMode mode = GoalMode(GoalModeType::VelocityWorld, + GoalModeType::VelocityWorld, + GoalModeType::AngleRate, + GoalModeType::PositionWorld); + return mode; + } + + static const GoalMode& getVelocityMode() + { + static const GoalMode mode = GoalMode(GoalModeType::VelocityWorld, + GoalModeType::VelocityWorld, + GoalModeType::AngleRate, + GoalModeType::VelocityWorld); + return mode; + } + + static const GoalMode& getPositionMode() + { + static const GoalMode mode = GoalMode(GoalModeType::PositionWorld, + GoalModeType::PositionWorld, + GoalModeType::AngleRate, + GoalModeType::PositionWorld); + return mode; + } + + static const GoalMode& getAllRateMode() + { + static const GoalMode mode = GoalMode(GoalModeType::AngleRate, + GoalModeType::AngleRate, + GoalModeType::AngleRate, + GoalModeType::Passthrough); + return mode; + } + + static const GoalMode& getUnknown() + { + static const GoalMode mode = GoalMode(GoalModeType::Unknown, + GoalModeType::Unknown, + GoalModeType::Unknown, + GoalModeType::Unknown); + return mode; + } +}; + +//config params for PID controller +template +struct PidConfig +{ + PidConfig(float kp_val = 0.01f, float ki_val = 0.0f, float kd_val = 0.0f, + T min_output_val = -1, T max_output_val = 1, + float time_scale_val = 1.0f / 1000, + bool enabled_val = true, T output_bias_val = T(), float iterm_discount_val = 1) + : kp(kp_val), ki(ki_val), kd(kd_val), time_scale(time_scale_val), min_output(min_output_val), max_output(max_output_val), enabled(enabled_val), output_bias(output_bias_val), iterm_discount(iterm_discount_val) + { + } + + float kp, ki, kd; + float time_scale; + T min_output, max_output; + bool enabled; + T output_bias; + float iterm_discount; + + enum class IntegratorType + { + Standard, + RungKutta + }; + IntegratorType integrator_type = IntegratorType::Standard; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IAxisController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IAxisController.hpp new file mode 100644 index 000000000..70b0c3a95 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IAxisController.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include "IUpdatable.hpp" +#include "IGoal.hpp" +#include "IStateEstimator.hpp" +#include "IBoardClock.hpp" + +namespace Rov_simple +{ + +class IAxisController : public IUpdatable +{ +public: + virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) = 0; + virtual TReal getOutput() = 0; + + virtual void reset() override + { + //disable checks for reset/update sequence because + //this object may get created but not used + clearResetUpdateAsserts(); + IUpdatable::reset(); + } +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoard.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoard.hpp new file mode 100644 index 000000000..d20653893 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoard.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "IUpdatable.hpp" +#include "IBoardClock.hpp" +#include "IBoardInputPins.hpp" +#include "IBoardOutputPins.hpp" +#include "IBoardSensors.hpp" + +namespace Rov_simple +{ + +class IBoard : public IUpdatable + , public IBoardClock + , public IBoardInputPins + , public IBoardOutputPins + , public IBoardSensors +{ +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardClock.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardClock.hpp new file mode 100644 index 000000000..2a4580732 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardClock.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +namespace Rov_simple +{ + +class IBoardClock +{ +public: + virtual uint64_t micros() const = 0; + virtual uint64_t millis() const = 0; + + virtual ~IBoardClock() = default; +}; +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardInputPins.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardInputPins.hpp new file mode 100644 index 000000000..a9e97d34a --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardInputPins.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +namespace Rov_simple +{ + +class IBoardInputPins +{ +public: + virtual float readChannel(uint16_t index) const = 0; //output -1 to 1 + virtual bool isRcConnected() const = 0; + virtual float getAvgMotorOutput() const = 0; + + virtual ~IBoardInputPins() = default; +}; +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardOutputPins.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardOutputPins.hpp new file mode 100644 index 000000000..8b8927e47 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardOutputPins.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +namespace Rov_simple +{ + +class IBoardOutputPins +{ +public: + virtual void writeOutput(uint16_t index, float val) = 0; //val = -1 to 1 for reversible motors otherwise 0 to 1 + virtual void setLed(uint8_t index, int32_t color) = 0; + + virtual ~IBoardOutputPins() = default; +}; +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardSensors.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardSensors.hpp new file mode 100644 index 000000000..291ca037d --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IBoardSensors.hpp @@ -0,0 +1,14 @@ +#pragma once + +namespace Rov_simple +{ + +class IBoardSensors +{ +public: + virtual void readAccel(float accel[3]) const = 0; //accel in m/s^2 + virtual void readGyro(float gyro[3]) const = 0; //angular velocity vector rad/sec + + virtual ~IBoardSensors() = default; +}; +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/ICommLink.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/ICommLink.hpp new file mode 100644 index 000000000..b1bdad998 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/ICommLink.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "IUpdatable.hpp" +#include + +namespace Rov_simple +{ + +class ICommLink : public IUpdatable +{ +public: + static constexpr int kLogLevelInfo = 0; + static constexpr int kLogLevelWarn = 1; + static constexpr int kLogLevelError = 2; + + virtual void log(const std::string& message, int32_t log_level = ICommLink::kLogLevelInfo) = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IController.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IController.hpp new file mode 100644 index 000000000..55944df88 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IController.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include "IUpdatable.hpp" +#include "IGoal.hpp" +#include "IStateEstimator.hpp" +#include "IBoardClock.hpp" + +namespace Rov_simple +{ + +class IController : public IUpdatable +{ +public: + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) = 0; + virtual const Axis4r& getOutput() = 0; + virtual bool isLastGoalModeAllPassthrough() = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IFirmware.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IFirmware.hpp new file mode 100644 index 000000000..771c9e215 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IFirmware.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include "IUpdatable.hpp" +#include "IOffboardApi.hpp" +#include "IStateEstimator.hpp" + +namespace Rov_simple +{ + +class IFirmware : public IUpdatable +{ +public: + virtual IOffboardApi& offboardApi() = 0; + virtual void overrideActuatorOutputs(const std::vector& values) = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IGoal.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IGoal.hpp new file mode 100644 index 000000000..8304e3adf --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IGoal.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "IUpdatable.hpp" +#include "CommonStructs.hpp" + +namespace Rov_simple +{ + +class IGoal +{ +public: + virtual const Axis4r& getGoalValue() const = 0; + virtual const GoalMode& getGoalMode() const = 0; + + virtual ~IGoal() = default; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IOffboardApi.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IOffboardApi.hpp new file mode 100644 index 000000000..f8cf57f15 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IOffboardApi.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include "CommonStructs.hpp" +#include "IGoal.hpp" + +namespace Rov_simple +{ + +class IOffboardApi : public IGoal +{ +public: + virtual bool canRequestApiControl(std::string& message) = 0; + virtual bool hasApiControl() = 0; + virtual bool requestApiControl(std::string& message) = 0; + virtual void releaseApiControl() = 0; + virtual bool setGoalAndMode(const Axis4r* goal, const GoalMode* goal_mode, std::string& message) = 0; + + virtual bool arm(std::string& message) = 0; + virtual bool disarm(std::string& message) = 0; + virtual VehicleStateType getVehicleState() const = 0; + + virtual bool getLandedState() const = 0; + + virtual const IStateEstimator& getStateEstimator() = 0; + virtual GeoPoint getHomeGeoPoint() const = 0; + virtual GeoPoint getGeoPoint() const = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IPidIntegrator.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IPidIntegrator.hpp new file mode 100644 index 000000000..26b34818e --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IPidIntegrator.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "CommonStructs.hpp" +#include + +namespace Rov_simple +{ + +template +class IPidIntegrator +{ +public: + virtual ~IPidIntegrator() {} + virtual void reset() = 0; + virtual void set(T val) = 0; + virtual void update(float dt, T error, uint64_t last_time) = 0; + virtual T getOutput() = 0; +}; + +} //namespace diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IStateEstimator.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IStateEstimator.hpp new file mode 100644 index 000000000..de648bae3 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IStateEstimator.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "CommonStructs.hpp" + +namespace Rov_simple +{ + +class IStateEstimator +{ +public: + virtual Axis3r getAngles() const = 0; + virtual Axis3r getAngularVelocity() const = 0; + virtual Axis3r getPosition() const = 0; + virtual Axis3r getLinearVelocity() const = 0; + virtual Axis4r getOrientation() const = 0; + virtual GeoPoint getGeoPoint() const = 0; + + virtual KinematicsState getKinematicsEstimated() const = 0; + + virtual GeoPoint getHomeGeoPoint() const = 0; + + virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; + + virtual ~IStateEstimator() = default; +}; +} diff --git a/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IUpdatable.hpp b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IUpdatable.hpp new file mode 100644 index 000000000..46d9d6433 --- /dev/null +++ b/AirLib/include/vehicles/rov/firmwares/rov_simple/firmware/interfaces/IUpdatable.hpp @@ -0,0 +1,36 @@ +#pragma once + +namespace Rov_simple +{ + +class IUpdatable +{ +public: + 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; + +protected: + void clearResetUpdateAsserts() + { + reset_called = false; + update_called = false; + } + +private: + bool reset_called = false; + bool update_called = false; +}; +} diff --git a/AirLib/src/vehicles/rov/api/RovApiBase.cpp b/AirLib/src/vehicles/rov/api/RovApiBase.cpp new file mode 100644 index 000000000..3672eaaa8 --- /dev/null +++ b/AirLib/src/vehicles/rov/api/RovApiBase.cpp @@ -0,0 +1,937 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +//in header only mode, control library is not available +#ifndef AIRLIB_HEADER_ONLY + +#include "vehicles/rov/api/RovApiBase.hpp" +#include +#include +#include +#include +#include + +namespace msr +{ +namespace airlib +{ + + void RovApiBase::resetImplementation() + { + cancelLastTask(); + SingleTaskCall lock(this); //cancel previous tasks + } + + bool RovApiBase::takeoff(float timeout_sec) + { + SingleTaskCall lock(this); + + auto kinematics = getKinematicsEstimated(); + if (kinematics.twist.linear.norm() > approx_zero_vel_) { + throw VehicleMoveException(Utils::stringf( + "Cannot perform takeoff because vehicle is already moving with velocity %f m/s", + kinematics.twist.linear.norm())); + } + + bool ret = moveToPosition(kinematics.pose.position.x(), + kinematics.pose.position.y(), + kinematics.pose.position.z() + getTakeoffZ(), + 0.5f, + timeout_sec, + DrivetrainType::MaxDegreeOfFreedom, + YawMode::Zero(), + -1, + 1); + + //last command is to hold on to position + //commandPosition(0, 0, getTakeoffZ(), YawMode::Zero()); + + return ret; + } + + bool RovApiBase::land(float timeout_sec) + { + SingleTaskCall lock(this); + + //after landing we detect if drone has stopped moving + int near_zero_vel_count = 0; + + return waitForFunction([&]() { + moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); + + float z_vel = getVelocity().z(); + if (z_vel <= approx_zero_vel_) + ++near_zero_vel_count; + else + near_zero_vel_count = 0; + + if (near_zero_vel_count > 10) + return true; + else { + moveByVelocityInternal(0, 0, landing_vel_, YawMode::Zero()); + return false; + } + }, + timeout_sec) + .isComplete(); + } + + bool RovApiBase::goHome(float timeout_sec) + { + SingleTaskCall lock(this); + + return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1); + } + + bool RovApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + float pitch, roll, yaw; + VectorMath::toEulerianAngle(getKinematicsEstimated().pose.orientation, pitch, roll, yaw); + float vx_new = (vx * (float)std::cos(yaw)) - (vy * (float)std::sin(yaw)); + float vy_new = (vx * (float)std::sin(yaw)) + (vy * (float)std::cos(yaw)); + + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx_new, vy_new, drivetrain, adj_yaw_mode); + + return waitForFunction([&]() { + moveByVelocityInternal(vx_new, vy_new, vz, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + float pitch, roll, yaw; + VectorMath::toEulerianAngle(getKinematicsEstimated().pose.orientation, pitch, roll, yaw); + float vx_new = (vx * (float)std::cos(yaw)) - (vy * (float)std::sin(yaw)); + float vy_new = (vx * (float)std::sin(yaw)) + (vy * (float)std::cos(yaw)); + + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx_new, vy_new, drivetrain, adj_yaw_mode); + + return waitForFunction([&]() { + moveByVelocityZInternal(vx_new, vy_new, z, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByMotorPWMs(const vector& pwm_values, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + bool res = waitForFunction([&]() { + commandMotorPWMs(pwm_values); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + + // Send a final command of zeros so it doesn't get stuck running last command + vector pwm_zeros(pwm_values.size(), 0.f); + commandMotorPWMs(pwm_zeros); + return res; + } + + bool RovApiBase::moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawZInternal(roll, pitch, yaw, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawThrottleInternal(roll, pitch, yaw, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateThrottleInternal(roll, pitch, yaw_rate, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByRollPitchYawrateZInternal(roll, pitch, yaw_rate, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesZInternal(roll_rate, pitch_rate, yaw_rate, z); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + return waitForFunction([&]() { + moveByAngleRatesThrottleInternal(roll_rate, pitch_rate, yaw_rate, throttle); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx, vy, drivetrain, adj_yaw_mode); + + return waitForFunction([&]() { + moveByVelocityInternal(vx, vy, vz, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return false; + + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adjustYaw(vx, vy, drivetrain, adj_yaw_mode); + + return waitForFunction([&]() { + moveByVelocityZInternal(vx, vy, z, adj_yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + bool RovApiBase::moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + + //validate path size + if (path.size() == 0) { + Utils::log("moveOnPath terminated because path has no points", Utils::kLogLevelWarn); + return true; + } + + //validate yaw mode + if (drivetrain == DrivetrainType::ForwardOnly && yaw_mode.is_rate) + throw std::invalid_argument("Yaw cannot be specified as rate if drivetrain is ForwardOnly"); + + //validate and set auto-lookahead value + float command_period_dist = velocity * getCommandPeriod(); + if (lookahead == 0) + throw std::invalid_argument("lookahead distance cannot be 0"); //won't allow progress on path + else if (lookahead > 0) { + if (command_period_dist > lookahead) + throw std::invalid_argument(Utils::stringf("lookahead value %f is too small for velocity %f. It must be at least %f", lookahead, velocity, command_period_dist)); + if (getDistanceAccuracy() > lookahead) + throw std::invalid_argument(Utils::stringf("lookahead value %f is smaller than drone's distance accuracy %f.", lookahead, getDistanceAccuracy())); + } + else { + //if auto mode requested for lookahead then calculate based on velocity + lookahead = getAutoLookahead(velocity, adaptive_lookahead); + Utils::log(Utils::stringf("lookahead = %f, adaptive_lookahead = %f", lookahead, adaptive_lookahead)); + } + + //add current position as starting point + vector path3d; + vector path_segs; + path3d.push_back(getKinematicsEstimated().pose.position); + + Vector3r point; + float path_length = 0; + //append the input path and compute segments + for (uint i = 0; i < path.size(); ++i) { + point = path.at(i); + PathSegment path_seg(path3d.at(i), point, velocity, path_length); + path_length += path_seg.seg_length; + path_segs.push_back(path_seg); + path3d.push_back(point); + } + //add last segment as zero length segment so we have equal number of segments and points. + //path_segs[i] refers to segment that starts at point i + path_segs.push_back(PathSegment(point, point, velocity, path_length)); + + //when path ends, we want to slow down + float breaking_dist = 0; + if (velocity > getRovApiParams().breaking_vel) { + breaking_dist = Utils::clip(velocity * getRovApiParams().vel_to_breaking_dist, + getRovApiParams().min_breaking_dist, + getRovApiParams().max_breaking_dist); + } + //else no need to change velocities for last segments + + //setup current position on path to 0 offset + PathPosition cur_path_loc, next_path_loc; + cur_path_loc.seg_index = 0; + cur_path_loc.offset = 0; + cur_path_loc.position = path3d[0]; + + float lookahead_error_increasing = 0; + float lookahead_error = 0; + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + + //initialize next path position + setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); + float overshoot = 0; + float goal_dist = 0; + + //until we are at the end of the path (last seg is always zero size) + while (!waiter.isTimeout() && (next_path_loc.seg_index < path_segs.size() - 1 || goal_dist > 0)) { //current position is approximately at the last end point + + float seg_velocity = path_segs.at(next_path_loc.seg_index).seg_velocity; + float path_length_remaining = path_length - path_segs.at(cur_path_loc.seg_index).seg_path_length - cur_path_loc.offset; + if (seg_velocity > getRovApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) { + seg_velocity = getRovApiParams().breaking_vel; + //Utils::logMessage("path_length_remaining = %f, Switched to breaking vel %f", path_length_remaining, seg_velocity); + } + + //send drone command to get to next lookahead + moveToPathPosition(next_path_loc.position, seg_velocity, drivetrain, yaw_mode, path_segs.at(cur_path_loc.seg_index).start_z); + + //sleep for rest of the cycle + if (!waiter.sleep()) + return false; + + /* Below, P is previous position on path, N is next goal and C is our current position. + + N + ^ + | + | + | + C'|---C + | / + | / + |/ + P + + Note that PC could be at any angle relative to PN, including 0 or -ve. We increase lookahead distance + by the amount of |PC|. For this, we project PC on to PN to get vector PC' and length of + CC'is our adaptive lookahead error by which we will increase lookahead distance. + + For next iteration, we first update our current position by goal_dist and then + set next goal by the amount lookahead + lookahead_error. + + We need to take care of following cases: + + 1. |PN| == 0 => lookahead_error = |PC|, goal_dist = 0 + 2. |PC| == 0 => lookahead_error = 0, goal_dist = 0 + 3. PC in opposite direction => lookahead_error = |PC|, goal_dist = 0 + + One good test case is if C just keeps moving perpendicular to the path (instead of along the path). + In that case, we expect next goal to come up and down by the amount of lookahead_error. However + under no circumstances we should go back on the path (i.e. current pos on path can only move forward). + */ + + //how much have we moved towards last goal? + const Vector3r& goal_vect = next_path_loc.position - cur_path_loc.position; + + if (!goal_vect.isZero()) { //goal can only be zero if we are at the end of path + const Vector3r& actual_vect = getPosition() - cur_path_loc.position; + + //project actual vector on goal vector + const Vector3r& goal_normalized = goal_vect.normalized(); + goal_dist = actual_vect.dot(goal_normalized); //dist could be -ve if drone moves away from goal + + //if adaptive lookahead is enabled the calculate lookahead error (see above fig) + if (adaptive_lookahead) { + const Vector3r& actual_on_goal = goal_normalized * goal_dist; + float error = (actual_vect - actual_on_goal).norm() * adaptive_lookahead; + if (error > lookahead_error) { + lookahead_error_increasing++; + //TODO: below should be lower than 1E3 and configurable + //but lower values like 100 doesn't work for simple_flight + ScalableClock + if (lookahead_error_increasing > 1E5) { + throw std::runtime_error("lookahead error is continually increasing so we do not have safe control, aborting moveOnPath operation"); + } + } + else { + lookahead_error_increasing = 0; + } + lookahead_error = error; + } + } + else { + lookahead_error_increasing = 0; + goal_dist = 0; + lookahead_error = 0; //this is not really required because we will exit + waiter.complete(); + } + + // Utils::logMessage("PF: cur=%s, goal_dist=%f, cur_path_loc=%s, next_path_loc=%s, lookahead_error=%f", + // VectorMath::toString(getPosition()).c_str(), goal_dist, VectorMath::toString(cur_path_loc.position).c_str(), + // VectorMath::toString(next_path_loc.position).c_str(), lookahead_error); + + //if drone moved backward, we don't want goal to move backward as well + //so only climb forward on the path, never back. Also note >= which means + //we climb path even if distance was 0 to take care of duplicated points on path + if (goal_dist >= 0) { + overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, goal_dist, cur_path_loc); + if (overshoot) + Utils::log(Utils::stringf("overshoot=%f", overshoot)); + } + //else + // Utils::logMessage("goal_dist was negative: %f", goal_dist); + + //compute next target on path + overshoot = setNextPathPosition(path3d, path_segs, cur_path_loc, lookahead + lookahead_error, next_path_loc); + } + + return waiter.isComplete(); + } + + bool RovApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + GeoPoint target; + target.latitude = latitude; + target.longitude = longitude; + target.altitude = altitude; + if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { + vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + else { + vector path{ Vector3r(getPosition().x(), getPosition().y(), getPosition().z()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + } + + bool RovApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + + vector path{ Vector3r(x, y, z) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + + bool RovApiBase::moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + + Vector2r cur_xy(getPosition().x(), getPosition().y()); + vector path{ Vector3r(cur_xy.x(), cur_xy.y(), z) }; + return moveOnPath(path, velocity, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead); + } + + bool RovApiBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) + { + SingleTaskCall lock(this); + + const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000; + + if (duration <= 0) + return true; + + //freeze the quaternion + Quaternionr starting_quaternion = getKinematicsEstimated().pose.orientation; + + Waiter waiter(getCommandPeriod(), duration, getCancelToken()); + do { + + RCData rc_data = getRCData(); + TTimeDelta age = clock()->elapsedSince(rc_data.timestamp); + if (rc_data.is_valid && (rc_data.timestamp == 0 || age <= kMaxMessageAge)) { //if rc message timestamp is not set OR is not too old + if (rc_data_trims_.is_valid) + rc_data.subtract(rc_data_trims_); + + //convert RC commands to velocity vector + const Vector3r vel_word(rc_data.pitch * vy_max / kMaxRCValue, rc_data.roll * vx_max / kMaxRCValue, 0); + Vector3r vel_body = VectorMath::transformToBodyFrame(vel_word, starting_quaternion, true); + + //find yaw as per terrain and remote setting + YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate); + adj_yaw_mode.yaw_or_rate += rc_data.yaw * 100.0f / kMaxRCValue; + adjustYaw(vel_body, drivetrain, adj_yaw_mode); + + //execute command + try { + float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z(); + moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode); + } + catch (const RovApiBase::UnsafeMoveException& ex) { + Utils::log(Utils::stringf("Safety violation: %s", ex.result.message.c_str()), Utils::kLogLevelWarn); + } + } + else + Utils::log(Utils::stringf("RCData had too old timestamp: %f", age)); + + } while (waiter.sleep()); + + //if timeout occurred then command completed successfully otherwise it was interrupted + return waiter.isTimeout(); + } + + bool RovApiBase::rotateToYaw(float yaw, float timeout_sec, float margin) + { + SingleTaskCall lock(this); + + if (timeout_sec <= 0) + return true; + + auto start_pos = getPosition(); + float yaw_target = VectorMath::normalizeAngle(yaw); + YawMode move_yaw_mode(false, yaw_target); + YawMode stop_yaw_mode(true, 0); + + return waitForFunction([&]() { + if (isYawWithinMargin(yaw_target, margin)) { // yaw is within margin, then trying to stop rotation + moveToPositionInternal(start_pos, stop_yaw_mode); // let yaw rate be zero + auto yaw_rate = getKinematicsEstimated().twist.angular.z(); + if (abs(yaw_rate) <= approx_zero_angular_vel_) { // already sopped + return true; //stop all for stably achieving the goal + } + } + else { // yaw is not within margin, go on rotation + moveToPositionInternal(start_pos, move_yaw_mode); + } + + // yaw is not within margin + return false; //keep moving until timeout + }, + timeout_sec) + .isComplete(); + } + + bool RovApiBase::rotateByYawRate(float yaw_rate, float duration) + { + SingleTaskCall lock(this); + + if (duration <= 0) + return true; + + auto start_pos = getPosition(); + YawMode yaw_mode(true, yaw_rate); + + return waitForFunction([&]() { + moveToPositionInternal(start_pos, yaw_mode); + return false; //keep moving until timeout + }, + duration) + .isTimeout(); + } + + void RovApiBase::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 2; + setControllerGains(controller_type, kp, ki, kd); + } + + void RovApiBase::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 3; + setControllerGains(controller_type, kp, ki, kd); + } + + void RovApiBase::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 4; + setControllerGains(controller_type, kp, ki, kd); + } + + void RovApiBase::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd) + { + uint8_t controller_type = 5; + setControllerGains(controller_type, kp, ki, kd); + } + + bool RovApiBase::hover() + { + SingleTaskCall lock(this); + + return moveToZ(getPosition().z(), 0.5f, Utils::max(), YawMode{ true, 0 }, 1.0f, false); + } + + void RovApiBase::moveByRC(const RCData& rc_data) + { + unused(rc_data); + //by default we say that this command is not supported + throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this Rov"); + } + + void RovApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode) + { + if (safetyCheckVelocity(Vector3r(vx, vy, vz))) + commandVelocity(vx, vy, vz, yaw_mode); + } + + void RovApiBase::moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode) + { + if (safetyCheckVelocityZ(vx, vy, z)) + commandVelocityZ(vx, vy, z, yaw_mode); + } + + void RovApiBase::moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode) + { + if (safetyCheckDestination(dest)) + commandPosition(dest.x(), dest.y(), dest.z(), yaw_mode); + } + + void RovApiBase::moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawZ(roll, pitch, yaw, z); + } + + void RovApiBase::moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawThrottle(roll, pitch, yaw, throttle); + } + + void RovApiBase::moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle); + } + + void RovApiBase::moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandRollPitchYawrateZ(roll, pitch, yaw_rate, z); + } + + void RovApiBase::moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z) + { + if (safetyCheckVelocity(getVelocity())) + commandAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z); + } + + void RovApiBase::moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle) + { + if (safetyCheckVelocity(getVelocity())) + commandAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle); + } + + //executes a given function until it returns true. Each execution is spaced apart at command period. + //return value is true if exit was due to given function returning true, otherwise false (due to timeout) + Waiter RovApiBase::waitForFunction(WaitFunction function, float timeout_sec) + { + Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken()); + if (timeout_sec <= 0) + return waiter; + + do { + if (function()) { + waiter.complete(); + break; + } + } while (waiter.sleep()); + return waiter; + } + + bool RovApiBase::waitForZ(float timeout_sec, float z, float margin) + { + float cur_z = 100000; + return waitForFunction([&]() { + cur_z = getPosition().z(); + return (std::abs(cur_z - z) <= margin); + }, + timeout_sec) + .isComplete(); + } + + void RovApiBase::setSafetyEval(const shared_ptr safety_eval_ptr) + { + SingleCall lock(this); + safety_eval_ptr_ = safety_eval_ptr; + } + + RCData RovApiBase::estimateRCTrims(float trimduration, float minCountForTrim, float maxTrim) + { + rc_data_trims_ = RCData(); + + //get trims + Waiter waiter_trim(getCommandPeriod(), trimduration, getCancelToken()); + uint count = 0; + do { + + const RCData rc_data = getRCData(); + if (rc_data.is_valid) { + rc_data_trims_.add(rc_data); + count++; + } + + } while (waiter_trim.sleep()); + + rc_data_trims_.is_valid = true; + + if (count < minCountForTrim) { + rc_data_trims_.is_valid = false; + Utils::log("Cannot compute RC trim because too few readings received"); + } + + //take average + rc_data_trims_.divideBy(static_cast(count)); + if (rc_data_trims_.isAnyMoreThan(maxTrim)) { + rc_data_trims_.is_valid = false; + Utils::log(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims_.toString().c_str())); + } + + Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims_.toString().c_str())); + + return rc_data_trims_; + } + + void RovApiBase::moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z) + { + unused(last_z); + //validate dest + if (dest.hasNaN()) + throw std::invalid_argument(VectorMath::toString(dest, "dest vector cannot have NaN: ")); + + //what is the distance we will travel at this velocity? + float expected_dist = velocity * getCommandPeriod(); + + //get velocity vector + const Vector3r cur = getPosition(); + const Vector3r cur_dest = dest - cur; + float cur_dest_norm = cur_dest.norm(); + + //yaw for the direction of travel + adjustYaw(cur_dest, drivetrain, yaw_mode); + + //find velocity vector + Vector3r velocity_vect; + if (cur_dest_norm < getDistanceAccuracy()) //our dest is approximately same as current + velocity_vect = Vector3r::Zero(); + else if (cur_dest_norm >= expected_dist) { + velocity_vect = (cur_dest / cur_dest_norm) * velocity; + //Utils::logMessage("velocity_vect=%s", VectorMath::toString(velocity_vect).c_str()); + } + else { //cur dest is too close than the distance we would travel + //generate velocity vector that is same size as cur_dest_norm / command period + //this velocity vect when executed for command period would yield cur_dest_norm + Utils::log(Utils::stringf("Too close dest: cur_dest_norm=%f, expected_dist=%f", cur_dest_norm, expected_dist)); + velocity_vect = (cur_dest / cur_dest_norm) * (cur_dest_norm / getCommandPeriod()); + } + + //send commands + //try to maintain altitude if path was in XY plan only, velocity based control is not as good + if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality + moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), 0, yaw_mode); + else + moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); + } + + bool RovApiBase::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z) + { + if (safety_eval_ptr_ == nullptr) + throw std::invalid_argument("The setSafety call requires safety_eval_ptr_ to be set first"); + + //default strategy is for move. In hover mode we set new strategy temporarily + safety_eval_ptr_->setSafety(enable_reasons, obs_clearance, obs_startegy, origin, xy_length, max_z, min_z); + + obs_avoidance_vel_ = obs_avoidance_vel; + Utils::log(Utils::stringf("obs_avoidance_vel: %f", obs_avoidance_vel_)); + + return true; + } + + bool RovApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result) + { + if (!result.is_safe) { + if (result.reason == SafetyEval::SafetyViolationType_::Obstacle) { + //are we supposed to do EM? + if (safety_eval_ptr_->getObsAvoidanceStrategy() != SafetyEval::ObsAvoidanceStrategy::RaiseException) { + //get suggested velocity vector + Vector3r avoidance_vel = getObsAvoidanceVelocity(result.cur_risk_dist, obs_avoidance_vel_) * result.suggested_vec; + + //use the unchecked command + commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z(), YawMode::Zero()); + + //tell caller not to execute planned command + return false; + } + //other wise throw exception + } + //otherwise there is some other reason why we are in unsafe situation + //send last command to come to full stop + commandVelocity(0, 0, 0, YawMode::Zero()); + throw UnsafeMoveException(result); + } + //else no unsafe situation + + return true; + } + + bool RovApiBase::safetyCheckVelocity(const Vector3r& velocity) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; + + const auto& result = safety_eval_ptr_->isSafeVelocity(getPosition(), velocity, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } + bool RovApiBase::safetyCheckVelocityZ(float vx, float vy, float z) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; + + const auto& result = safety_eval_ptr_->isSafeVelocityZ(getPosition(), vx, vy, z, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } + bool RovApiBase::safetyCheckDestination(const Vector3r& dest_pos) + { + if (safety_eval_ptr_ == nullptr) //safety checks disabled + return true; + + const auto& result = safety_eval_ptr_->isSafeDestination(getPosition(), dest_pos, getOrientation()); + return emergencyManeuverIfUnsafe(result); + } + + float RovApiBase::setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc) + { + //note: cur_path_loc and next_path_loc may both point to same object + uint i = cur_path_loc.seg_index; + float offset = cur_path_loc.offset; + while (i < path.size() - 1) { + const PathSegment& seg = path_segs.at(i); + if (seg.seg_length > 0 && //protect against duplicate points in path, normalized seg will have NaN + seg.seg_length >= next_dist + offset) { + + next_path_loc.seg_index = i; + next_path_loc.offset = next_dist + offset; //how much total distance we will travel on this segment + next_path_loc.position = path.at(i) + seg.seg_normalized * next_path_loc.offset; + return 0; + } + //otherwise use up this segment, move on to next one + next_dist -= seg.seg_length - offset; + offset = 0; + + if (&cur_path_loc == &next_path_loc) + Utils::log(Utils::stringf("segment %d done: x=%f, y=%f, z=%f", i, path.at(i).x(), path.at(i).y(), path.at(i).z())); + + ++i; + } + + //if we are here then we ran out of segments + //consider last segment as zero length segment + next_path_loc.seg_index = i; + next_path_loc.offset = 0; + next_path_loc.position = path.at(i); + return next_dist; + } + + void RovApiBase::adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode) + { + //adjust yaw for the direction of travel in forward-only mode + if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { + if (heading.norm() > getDistanceAccuracy()) { + yaw_mode.yaw_or_rate = yaw_mode.yaw_or_rate + (std::atan2(heading.y(), heading.x()) * 180 / M_PIf); + yaw_mode.yaw_or_rate = VectorMath::normalizeAngle(yaw_mode.yaw_or_rate); + } + else + yaw_mode.setZeroRate(); //don't change existing yaw if heading is too small because that can generate random result + } + //else no adjustment needed + } + + void RovApiBase::adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode) + { + adjustYaw(Vector3r(x, y, 0), drivetrain, yaw_mode); + } + + bool RovApiBase::isYawWithinMargin(float yaw_target, float margin) const + { + const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; + return std::abs(yaw_current - yaw_target) <= margin; + } + + float RovApiBase::getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor, float min_factor) const + { + //if auto mode requested for lookahead then calculate based on velocity + float command_period_dist = velocity * getCommandPeriod(); + float lookahead = command_period_dist * (adaptive_lookahead > 0 ? min_factor : max_factor); + lookahead = std::max(lookahead, getDistanceAccuracy() * 1.5f); //50% more than distance accuracy + return lookahead; + } + + float RovApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const + { + unused(risk_dist); + return max_obs_avoidance_vel; + } +} +} //namespace +#endif diff --git a/AirLib/src/vehicles/rov/api/RovRpcLibClient.cpp b/AirLib/src/vehicles/rov/api/RovRpcLibClient.cpp new file mode 100644 index 000000000..6c88c385c --- /dev/null +++ b/AirLib/src/vehicles/rov/api/RovRpcLibClient.cpp @@ -0,0 +1,273 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +//in header only mode, control library is not available +#ifndef AIRLIB_HEADER_ONLY +//RPC code requires C++14. If build system like Unreal doesn't support it then use compiled binaries +#ifndef AIRLIB_NO_RPC +//if using Unreal Build system then include precompiled header file first + +#include "vehicles/rov/api/RovRpcLibClient.hpp" + +#include "common/Common.hpp" +#include +STRICT_MODE_OFF + +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK + +#ifdef nil +#undef nil +#endif // nil + +#include "common/common_utils/WindowsApisCommonPre.hpp" +#undef FLOAT +#undef check +#include "rpc/client.h" +//TODO: HACK: UE4 defines macro with stupid names like "check" that conflicts with msgpack library +#ifndef check +#define check(expr) (static_cast((expr))) +#endif +#include "common/common_utils/WindowsApisCommonPost.hpp" + +#include "vehicles/rov/api/RovRpcLibAdaptors.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibAdaptors.hpp" + +STRICT_MODE_ON +#ifdef _MSC_VER +__pragma(warning(disable : 4239)) +#endif + + namespace msr +{ + namespace airlib + { + + typedef msr::airlib_rpclib::RovRpcLibAdaptors RovRpcLibAdaptors; + typedef msr::airlib_rpclib::MultirotorRpcLibAdaptors MultirotorRpcLibAdaptors; + + struct RovRpcLibClient::impl + { + public: + std::future last_future; + }; + + RovRpcLibClient::RovRpcLibClient(const string& ip_address, uint16_t port, float timeout_sec) + : RpcLibClientBase(ip_address, port, timeout_sec) + { + pimpl_.reset(new impl()); + } + + RovRpcLibClient::~RovRpcLibClient() + { + } + + RovRpcLibClient* RovRpcLibClient::takeoffAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("takeoff", timeout_sec, vehicle_name); + return this; + } + RovRpcLibClient* RovRpcLibClient::landAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("land", timeout_sec, vehicle_name); + return this; + } + RovRpcLibClient* RovRpcLibClient::goHomeAsync(float timeout_sec, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("goHome", timeout_sec, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZBodyFrame", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByMotorPWMsAsync(const vector& pwm_values, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByMotorPWMs", pwm_values, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawZ", roll, pitch, yaw, z, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawThrottle", roll, pitch, yaw, throttle, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByRollPitchYawrateThrottleAsync(float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateThrottle", roll, pitch, yaw_rate, throttle, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByRollPitchYawrateZAsync(float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByRollPitchYawrateZ", roll, pitch, yaw_rate, z, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByAngleRatesZAsync(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesZ", roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByVelocityAsync(float vx, float vy, float vz, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveOnPathAsync(const vector& path, float velocity, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + vector conv_path; + RovRpcLibAdaptors::from(path, conv_path); + pimpl_->last_future = static_cast(getClient())->async_call("moveOnPath", conv_path, velocity, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, + DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveToZ", z, velocity, timeout_sec, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::rotateToYawAsync(float yaw, float timeout_sec, float margin, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("rotateToYaw", yaw, timeout_sec, margin, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::rotateByYawRateAsync(float yaw_rate, float duration, const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("rotateByYawRate", yaw_rate, duration, vehicle_name); + return this; + } + + RovRpcLibClient* RovRpcLibClient::hoverAsync(const std::string& vehicle_name) + { + pimpl_->last_future = static_cast(getClient())->async_call("hover", vehicle_name); + return this; + } + + void RovRpcLibClient::setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setAngleLevelControllerGains", kp, ki, kd, vehicle_name); + } + + void RovRpcLibClient::setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setAngleRateControllerGains", kp, ki, kd, vehicle_name); + } + + void RovRpcLibClient::setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setVelocityControllerGains", kp, ki, kd, vehicle_name); + } + + void RovRpcLibClient::setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) + { + static_cast(getClient())->call("setPositionControllerGains", kp, ki, kd, vehicle_name); + } + + bool RovRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) + { + return static_cast(getClient())->call("setSafety", static_cast(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, RovRpcLibAdaptors::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as(); + } + + //status getters + // Rotor state getter + RotorTiltableStates RovRpcLibClient::getRotorStates(const std::string& vehicle_name) + { + return static_cast(getClient())->call("getRotorStates", vehicle_name).as().to(); + } + // Rov state getter + RovState RovRpcLibClient::getRovState(const std::string& vehicle_name) + { + return static_cast(getClient())->call("getRovState", vehicle_name).as().to(); + } + + void RovRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name) + { + static_cast(getClient())->call("moveByRC", RovRpcLibAdaptors::RCData(rc_data), vehicle_name); + } + + //return value of last task. It should be true if task completed without + //cancellation or timeout + RovRpcLibClient* RovRpcLibClient::waitOnLastTask(bool* task_result, float timeout_sec) + { + bool result; + if (std::isnan(timeout_sec) || timeout_sec == Utils::max()) + result = pimpl_->last_future.get().as(); + else { + auto future_status = pimpl_->last_future.wait_for(std::chrono::duration(timeout_sec)); + if (future_status == std::future_status::ready) + result = pimpl_->last_future.get().as(); + else + result = false; + } + + if (task_result) + *task_result = result; + + return this; + } + } +} //namespace + +#endif +#endif diff --git a/AirLib/src/vehicles/rov/api/RovRpcLibServer.cpp b/AirLib/src/vehicles/rov/api/RovRpcLibServer.cpp new file mode 100644 index 000000000..e81b5aa96 --- /dev/null +++ b/AirLib/src/vehicles/rov/api/RovRpcLibServer.cpp @@ -0,0 +1,159 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +//in header only mode, control library is not available +#ifndef AIRLIB_HEADER_ONLY +//RPC code requires C++14. If build system like Unreal doesn't support it then use compiled binaries +#ifndef AIRLIB_NO_RPC +//if using Unreal Build system then include pre-compiled header file first + +#include "vehicles/rov/api/RovRpcLibServer.hpp" + +#include "common/Common.hpp" +STRICT_MODE_OFF + +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +#include "common/common_utils/MinWinDefines.hpp" +#undef NOUSER + +#include "common/common_utils/WindowsApisCommonPre.hpp" +#undef FLOAT +#undef check +#include "rpc/server.h" +//TODO: HACK: UE4 defines macro with stupid names like "check" that conflicts with msgpack library +#ifndef check +#define check(expr) (static_cast((expr))) +#endif +#include "common/common_utils/WindowsApisCommonPost.hpp" + +#include "vehicles/rov/api/RovRpcLibAdaptors.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibAdaptors.hpp" + +STRICT_MODE_ON + +namespace msr +{ +namespace airlib +{ + + typedef msr::airlib_rpclib::RovRpcLibAdaptors RovRpcLibAdaptors; + typedef msr::airlib_rpclib::MultirotorRpcLibAdaptors MultirotorRpcLibAdaptors; + + RovRpcLibServer::RovRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port) + : RpcLibServerBase(api_provider, server_address, port) + { + (static_cast(getServer()))->bind("takeoff", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->takeoff(timeout_sec); + }); + (static_cast(getServer()))->bind("land", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->land(timeout_sec); + }); + (static_cast(getServer()))->bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->goHome(timeout_sec); + }); + (static_cast(getServer()))->bind("moveByVelocityBodyFrame", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityBodyFrame(vx, vy, vz, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveByVelocityZBodyFrame", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityZBodyFrame(vx, vy, z, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveByMotorPWMs", [&](const vector& pwm_values, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByMotorPWMs(pwm_values, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawZ", [&](float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawZ(roll, pitch, yaw, z, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawThrottle", [&](float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawThrottle(roll, pitch, yaw, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawrateThrottle", [&](float roll, float pitch, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateThrottle(roll, pitch, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByRollPitchYawrateZ", [&](float roll, float pitch, float yaw_rate, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByRollPitchYawrateZ(roll, pitch, yaw_rate, z, duration); + }); + (static_cast(getServer()))->bind("moveByAngleRatesZ", [&](float roll_rate, float pitch_rate, float yaw_rate, float z, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesZ(roll_rate, pitch_rate, yaw_rate, z, duration); + }); + (static_cast(getServer()))->bind("moveByAngleRatesThrottle", [&](float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByAngleRatesThrottle(roll_rate, pitch_rate, yaw_rate, throttle, duration); + }); + (static_cast(getServer()))->bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveByVelocityZ", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); + }); + (static_cast(getServer()))->bind("moveOnPath", [&](const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + vector conv_path; + RovRpcLibAdaptors::to(path, conv_path); + return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToPosition(x, y, z, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveToZ", [&](float z, float velocity, float timeout_sec, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToZ(z, velocity, timeout_sec, yaw_mode.to(), lookahead, adaptive_lookahead); + }); + (static_cast(getServer()))->bind("moveByManual", [&](float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByManual(vx_max, vy_max, z_min, duration, drivetrain, yaw_mode.to()); + }); + + (static_cast(getServer()))->bind("rotateToYaw", [&](float yaw, float timeout_sec, float margin, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->rotateToYaw(yaw, timeout_sec, margin); + }); + (static_cast(getServer()))->bind("rotateByYawRate", [&](float yaw_rate, float duration, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->rotateByYawRate(yaw_rate, duration); + }); + (static_cast(getServer()))->bind("hover", [&](const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->hover(); + }); + (static_cast(getServer()))->bind("setAngleLevelControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleLevelControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))->bind("setAngleRateControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setAngleRateControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))->bind("setVelocityControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setVelocityControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))->bind("setPositionControllerGains", [&](const vector& kp, const vector& ki, const vector& kd, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->setPositionControllerGains(kp, ki, kd); + }); + (static_cast(getServer()))->bind("moveByRC", [&](const RovRpcLibAdaptors::RCData& data, const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->moveByRC(data.to()); + }); + + (static_cast(getServer()))->bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy, float obs_avoidance_vel, const RovRpcLibAdaptors::Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); + }); + + (static_cast(getServer()))->bind("simSetRovPose", [&](const RovRpcLibAdaptors::Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props, const std::string& vehicle_name) -> void { + getVehicleSimApi(vehicle_name)->setPoseCustom(pose.to(), tilt_angles, ignore_collision, spin_props); + }); + + //getters + // Rotor state + (static_cast(getServer()))->bind("getRotorStates", [&](const std::string& vehicle_name) -> RovRpcLibAdaptors::RotorTiltableStates { + return RovRpcLibAdaptors::RotorTiltableStates(getVehicleApi(vehicle_name)->getRotorStates()); + }); + // Rov state + (static_cast(getServer()))->bind("getRovState", [&](const std::string& vehicle_name) -> RovRpcLibAdaptors::RovState { + return RovRpcLibAdaptors::RovState(getVehicleApi(vehicle_name)->getRovState()); + }); + } + + //required for pimpl + RovRpcLibServer::~RovRpcLibServer() + { + } +} +} //namespace + +#endif +#endif diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 4f139ac55..bda542b65 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -1618,4 +1618,104 @@ def getCarControls(self, vehicle_name=''): CarControls: """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) + +#----------------------------------- ROV APIs --------------------------------------------- +class RovClient(VehicleClient, object): + def __init__(self, ip = "", port = 41451, timeout_value = 3600): + super(RovClient, self).__init__(ip, port, timeout_value) + + def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): + return self.client.call_async('takeoff', timeout_sec, vehicle_name) + + def landAsync(self, timeout_sec = 60, vehicle_name = ''): + return self.client.call_async('land', timeout_sec, vehicle_name) + + def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): + return self.client.call_async('goHome', timeout_sec, vehicle_name) + + def hoverAsync(self, vehicle_name = ''): + return self.client.call_async('hover', vehicle_name) + + def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityBodyFrame', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByVelocityZBodyFrameAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityZBodyFrame', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByMotorPWMsAsync(self, pwm_values, duration, vehicle_name = ''): + return self.client.call_async('moveByMotorPWMs', pwm_values, duration, vehicle_name) + + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawZ', roll, pitch, yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawThrottle', roll, pitch, yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, pitch, yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawrateZ', roll, pitch, yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleRatesZ', roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name) + + def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + + def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, + yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, + yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, vehicle_name) + + def rotateToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''): + return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name) + + def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''): + return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name) + + def setAngleLevelControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setAngleLevelControllerGains', kp, ki, kd, vehicle_name) + + def setAngleRateControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setAngleRateControllerGains', kp, ki, kd, vehicle_name) + + def setVelocityControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setVelocityControllerGains', kp, ki, kd, vehicle_name) + + def setPositionControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setPositionControllerGains', kp, ki, kd, vehicle_name) + + def moveByRC(self, rc_data, vehicle_name = ''): + self.client.call('moveByRC', rc_data, vehicle_name) + + def simSetRovPose(self, pose, tilt_angles, ignore_collision = True, spin_props = False, vehicle_name = ''): + self.client.call('simSetRovPose', pose, tilt_angles, ignore_collision, spin_props, vehicle_name) + + def getRotorStates(self, vehicle_name = ''): + return RotorTiltableStates.from_msgpack(self.client.call('getRotorStates', vehicle_name)) + getRotorStates.__annotations__ = {'return': RotorTiltableStates} + + def getRovState(self, vehicle_name = ''): + return RovState.from_msgpack(self.client.call('getRovState', vehicle_name)) + getRovState.__annotations__ = {'return': RovState} \ No newline at end of file diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 7aef00554..89968d60a 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -578,3 +578,25 @@ class MeshPositionVertexBuffersResponse(MsgpackMixin): vertices = 0.0 indices = 0.0 name = '' + +class RotorTiltableParameters(MsgpackMixin): + thrust = 0.0 + torque_scaler = 0.0 + speed = 0.0 + angle = 0.0 + +class RotorTiltableStates(MsgpackMixin): + timestamp = np.uint64(0) + rotors = [] + +class RovState(MsgpackMixin): + collision = CollisionInfo() + kinematics_estimated = KinematicsState() + kinematics_true = KinematicsState() + gps_location = GeoPoint() + timestamp = np.uint64(0) + landed_state = LandedState.Landed + rc_data = RCData() + ready = False + ready_message = "" + can_arm = False diff --git a/PythonClient/rov/hello_rov.py b/PythonClient/rov/hello_rov.py new file mode 100644 index 000000000..77ac44c09 --- /dev/null +++ b/PythonClient/rov/hello_rov.py @@ -0,0 +1,59 @@ +import setup_path +import airsim +import time +import pprint + +def main(): + # Connect to the AirSim simulator (ROV) + client = airsim.RovClient() + print("Connecting to AirSim ROV...") + client.confirmConnection() + client.enableApiControl(True) + + print("Arming the ROV...") + client.armDisarm(True) + + # Query vehicle state + state = client.getRovState() + print("ROV State:\n%s" % pprint.pformat(state)) + + # Query sensor states + try: + imu_data = client.getImuData() + print("IMU Data:\n%s" % pprint.pformat(imu_data)) + except Exception as e: + print("IMU query failed: %s" % e) + + try: + baro_data = client.getBarometerData() + print("Barometer (Depth) Data:\n%s" % pprint.pformat(baro_data)) + except Exception as e: + print("Barometer query failed: %s" % e) + + try: + mag_data = client.getMagnetometerData() + print("Magnetometer Data:\n%s" % pprint.pformat(mag_data)) + except Exception as e: + print("Magnetometer query failed: %s" % e) + + # Basic movement test: dive 2 meters and move forward + print("Moving ROV by velocity in body frame (forward 0.5 m/s, dive 0.2 m/s for 3s)...") + client.moveByVelocityBodyFrameAsync(vx=0.5, vy=0.0, vz=0.2, duration=3).join() + + time.sleep(1) + + state = client.getRovState() + print("Updated ROV Position: %s" % state.kinematics_estimated.position) + + # Hover in place + print("Hovering...") + client.hoverAsync().join() + + # Disarm and release control + print("Disarming and resetting...") + client.armDisarm(False) + client.enableApiControl(False) + print("Done!") + +if __name__ == "__main__": + main() diff --git a/PythonClient/rov/setup_path.py b/PythonClient/rov/setup_path.py new file mode 100644 index 000000000..4b2e2ec13 --- /dev/null +++ b/PythonClient/rov/setup_path.py @@ -0,0 +1,40 @@ +# Import this module to automatically setup path to local airsim module +import os, sys, logging + +class SetupPath: + @staticmethod + def getDirLevels(path): + path_norm = os.path.normpath(path) + return len(path_norm.split(os.sep)) + + @staticmethod + def getCurrentPath(): + cur_filepath = __file__ + return os.path.dirname(cur_filepath) + + @staticmethod + def getGrandParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 2: + return os.path.dirname(os.path.dirname(cur_path)) + return '' + + @staticmethod + def getParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 1: + return os.path.dirname(cur_path) + return '' + + @staticmethod + def addAirSimModulePath(): + parent = SetupPath.getParentDir() + if parent != '': + airsim_path = os.path.join(parent, 'airsim') + client_path = os.path.join(airsim_path, 'client.py') + if os.path.exists(client_path): + sys.path.insert(0, parent) + else: + logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).") + +SetupPath.addAirSimModulePath() diff --git a/Unreal/Environments/Blocks/Blocks.uproject b/Unreal/Environments/Blocks/Blocks.uproject index 004438bc7..d3352abfb 100644 --- a/Unreal/Environments/Blocks/Blocks.uproject +++ b/Unreal/Environments/Blocks/Blocks.uproject @@ -1,6 +1,6 @@ { "FileVersion": 3, - "EngineAssociation": "{B72AF44D-4F3F-E2CD-BBAF-408A9ABCEF7F}", + "EngineAssociation": "{8E0583E3-4EF2-9694-09C9-B7B5E924A9F9}", "Category": "", "Description": "", "Modules": [ diff --git a/Unreal/Environments/Blocks/Config/DefaultEngine.ini b/Unreal/Environments/Blocks/Config/DefaultEngine.ini index 7fe134840..165ac760c 100644 --- a/Unreal/Environments/Blocks/Config/DefaultEngine.ini +++ b/Unreal/Environments/Blocks/Config/DefaultEngine.ini @@ -68,4 +68,16 @@ AsyncSceneSmoothingFactor=0.990000 InitialAverageFrameRate=0.016667 PhysXTreeRebuildRate=10 +[ConsoleVariables] +rhi.ResourceTableCaching=0 + +[/Script/Engine.RendererSettings] +r.TextureStreaming=False + +[/Script/WindowsTargetPlatform.WindowsTargetSettings] +DefaultGraphicsRHI=DefaultGraphicsRHI_DX12 + + + + diff --git a/Unreal/Environments/Blocks/Content/Airpuff.uasset b/Unreal/Environments/Blocks/Content/Airpuff.uasset new file mode 100644 index 000000000..f5b9caf5d Binary files /dev/null and b/Unreal/Environments/Blocks/Content/Airpuff.uasset differ diff --git a/Unreal/Environments/Blocks/Content/Deafult.uasset b/Unreal/Environments/Blocks/Content/Deafult.uasset new file mode 100644 index 000000000..2a265fda9 Binary files /dev/null and b/Unreal/Environments/Blocks/Content/Deafult.uasset differ diff --git a/Unreal/Environments/Blocks/Content/Motor.uasset b/Unreal/Environments/Blocks/Content/Motor.uasset new file mode 100644 index 000000000..62531848f Binary files /dev/null and b/Unreal/Environments/Blocks/Content/Motor.uasset differ diff --git a/Unreal/Environments/Blocks/Content/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset b/Unreal/Environments/Blocks/Content/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset new file mode 100644 index 000000000..197700bb5 Binary files /dev/null and b/Unreal/Environments/Blocks/Content/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset differ diff --git a/Unreal/Environments/Blocks/Content/glass.uasset b/Unreal/Environments/Blocks/Content/glass.uasset new file mode 100644 index 000000000..174ec28d2 Binary files /dev/null and b/Unreal/Environments/Blocks/Content/glass.uasset differ diff --git a/Unreal/Environments/Blocks/settings.json b/Unreal/Environments/Blocks/settings.json index f071345af..ee39e63c7 100644 --- a/Unreal/Environments/Blocks/settings.json +++ b/Unreal/Environments/Blocks/settings.json @@ -1,4 +1,45 @@ { + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", "SettingsVersion": 1.2, - "SimMode": "Multirotor" -} \ No newline at end of file + "SimMode": "Rov", + "ClockSpeed": 1, + "PawnPaths": { + "DefaultQuadrotor": {"PawnBP": "Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'"}, + "DefaultRov": {"PawnBP": "Class'/Script/AirSim.RovPawn'"}, + "DefaultComputerVision": {"PawnBP": "Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'"} + }, + "Vehicles": { + "RovSimple": { + "VehicleType": "RovSimple", + "DefaultVehicleState": "Armed", + "PawnPath": "DefaultRov", + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + }, + "Cameras": { + "front_right_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.50, "Y": 0.06, "Z": 0.10, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + } + } + + + + + } + } + +} + diff --git a/Unreal/Environments/Blocks/update_from_git.bat b/Unreal/Environments/Blocks/update_from_git.bat index 9d8654036..6e32f47d9 100644 --- a/Unreal/Environments/Blocks/update_from_git.bat +++ b/Unreal/Environments/Blocks/update_from_git.bat @@ -17,6 +17,12 @@ echo Using AirSimPath = %AirSimPath% robocopy /MIR "%AirSimPath%\Unreal\Plugins\AirSim" Plugins\AirSim /XD temp *. /njh /njs /ndl /np robocopy /MIR "%AirSimPath%\AirLib" Plugins\AirSim\Source\AirLib /XD temp *. /njh /njs /ndl /np + +if not exist Plugins\Carla ( + if exist Plugins\AirSim\Source\SimWorldGameMode.h ren Plugins\AirSim\Source\SimWorldGameMode.h SimWorldGameMode.h.disabled + if exist Plugins\AirSim\Source\SimWorldGameMode.cpp ren Plugins\AirSim\Source\SimWorldGameMode.cpp SimWorldGameMode.cpp.disabled +) + robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.bat robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.sh rem robocopy /njh /njs /ndl /np "%AirSimPath%" "." *.gitignore diff --git a/Unreal/Environments/Blocks/update_from_git_ci.bat b/Unreal/Environments/Blocks/update_from_git_ci.bat index 58c565c83..ca4c82ff2 100644 --- a/Unreal/Environments/Blocks/update_from_git_ci.bat +++ b/Unreal/Environments/Blocks/update_from_git_ci.bat @@ -17,6 +17,12 @@ echo Using AirSimPath = %AirSimPath% robocopy /MIR "%AirSimPath%\Unreal\Plugins\AirSim" Plugins\AirSim /XD temp *. /njh /njs /ndl /np robocopy /MIR "%AirSimPath%\AirLib" Plugins\AirSim\Source\AirLib /XD temp *. /njh /njs /ndl /np + +if not exist Plugins\Carla ( + if exist Plugins\AirSim\Source\SimWorldGameMode.h ren Plugins\AirSim\Source\SimWorldGameMode.h SimWorldGameMode.h.disabled + if exist Plugins\AirSim\Source\SimWorldGameMode.cpp ren Plugins\AirSim\Source\SimWorldGameMode.cpp SimWorldGameMode.cpp.disabled +) + robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.bat robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.sh rem robocopy /njh /njs /ndl /np "%AirSimPath%" "." *.gitignore diff --git a/Unreal/Plugins/AirSim/AirSim.uplugin b/Unreal/Plugins/AirSim/AirSim.uplugin index 86aac01e4..5e28779f3 100644 --- a/Unreal/Plugins/AirSim/AirSim.uplugin +++ b/Unreal/Plugins/AirSim/AirSim.uplugin @@ -24,7 +24,8 @@ "Plugins": [ { "Name": "Carla", - "Enabled": true + "Enabled": true, + "Optional": true }, { "Name": "PhysXVehicles", diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/Airpuff.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/Airpuff.uasset new file mode 100644 index 000000000..f5b9caf5d Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/Airpuff.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/Deafult.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/Deafult.uasset new file mode 100644 index 000000000..2a265fda9 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/Deafult.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/Motor.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/Motor.uasset new file mode 100644 index 000000000..62531848f Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/Motor.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2.uasset new file mode 100644 index 000000000..197700bb5 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset new file mode 100644 index 000000000..197700bb5 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Models/RoV/glass.uasset b/Unreal/Plugins/AirSim/Content/Models/RoV/glass.uasset new file mode 100644 index 000000000..174ec28d2 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Models/RoV/glass.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/bluerovheavy2.fbx b/Unreal/Plugins/AirSim/Content/bluerovheavy2.fbx new file mode 100644 index 000000000..24d3c3f80 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/bluerovheavy2.fbx differ diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index cb30e3e21..5b695885f 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -741,6 +741,14 @@ UClass* UAirBlueprintLib::LoadClass(const std::string& name) { FString str(name.c_str()); UClass* cls = StaticLoadClass(UObject::StaticClass(), nullptr, *str); + if (cls == nullptr) { + cls = FindObject(ANY_PACKAGE, *str); + if (!cls) { + FString CleanName = FPackageName::ObjectPathToObjectName(str); + CleanName.RemoveFromEnd(TEXT("_C")); + cls = FindObject(ANY_PACKAGE, *CleanName); + } + } if (cls == nullptr) { std::string msg = "Failed to load asset class - " + name; FString fmsg(msg.c_str()); diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 45600e11e..59cd29b00 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -79,7 +79,22 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape", "CinematicCamera" }); - PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Carla", "Foliage" }); + PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Foliage" }); + + string ProjectDir = Target.ProjectFile != null ? Path.GetDirectoryName(Target.ProjectFile.FullName) : ""; + string CarlaPluginInProject = Path.Combine(ProjectDir, "Plugins", "Carla"); + string CarlaPluginSibling = Path.Combine(AirSimPluginPath, "..", "Carla"); + bool bHaveCarla = Directory.Exists(CarlaPluginInProject) || Directory.Exists(CarlaPluginSibling); + + if (bHaveCarla) + { + PrivateDependencyModuleNames.Add("Carla"); + PublicDefinitions.Add("WITH_CARLA=1"); + } + else + { + PublicDefinitions.Add("WITH_CARLA=0"); + } //suppress VC++ proprietary warnings PublicDefinitions.Add("_SCL_SECURE_NO_WARNINGS=1"); diff --git a/Unreal/Plugins/AirSim/Source/AirSim.cpp b/Unreal/Plugins/AirSim/Source/AirSim.cpp index 8fbbc514b..042fa9fe2 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.cpp +++ b/Unreal/Plugins/AirSim/Source/AirSim.cpp @@ -5,7 +5,7 @@ #include "Misc/Paths.h" #include "Modules/ModuleManager.h" #include "Modules/ModuleInterface.h" -#if WITH_EDITOR +#if WITH_EDITOR && WITH_CARLA #include #include #include @@ -31,7 +31,7 @@ void FAirSim::ShutdownModule() //plugin shutdown } -#if WITH_EDITOR +#if WITH_EDITOR && WITH_CARLA namespace carla { diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 5b8f8ba53..f6e0d12b0 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -6,6 +6,7 @@ #include "Vehicles/Multirotor/SimModeWorldMultiRotor.h" #include "Vehicles/Car/SimModeCar.h" #include "Vehicles/ComputerVision/SimModeComputerVision.h" +#include "Vehicles/Rov/SimModeWorldRov.h" #include "common/AirSimSettings.hpp" #include @@ -279,6 +280,10 @@ void ASimHUD::createSimMode() simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeRov) + simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, + FRotator::ZeroRotator, + simmode_spawn_params); else { UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, std::string("SimMode is not valid: ") + simmode_name, "Error"); UAirBlueprintLib::LogMessageString("SimMode is not valid: ", simmode_name, LogDebugLevel::Failure); @@ -332,9 +337,10 @@ FString ASimHUD::getLaunchPath(const std::string& filename) bool ASimHUD::getSettingsText(std::string& settingsText) { return (getSettingsTextFromCommandLine(settingsText) || - readSettingsTextFromFile(FString(msr::airlib::Settings::getExecutableFullPath("settings.json").c_str()), settingsText) || + readSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || + readSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::getExecutableFullPath("settings.json").c_str()), settingsText) || readSettingsTextFromFile(getLaunchPath("settings.json"), settingsText) || - readSettingsTextFromFile(FString(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); + readSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); } // Attempts to parse the settings file path or the settings text from the command line diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 943cece85..3ebd6fe02 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -162,6 +162,8 @@ class AIRSIM_API ASimModeBase : public AActor UClass* pip_camera_class; UPROPERTY() UParticleSystem* collision_display_template; + UPROPERTY() + TArray spawned_actors_; //keep refs alive from Unreal GC private: typedef common_utils::Utils Utils; @@ -207,9 +209,6 @@ class AIRSIM_API ASimModeBase : public AActor common_utils::UniqueValueMap external_cameras_; std::unique_ptr external_image_capture_; - UPROPERTY() - TArray spawned_actors_; //keep refs alive from Unreal GC - bool lidar_checks_done_ = false; bool lidar_draw_debug_points_ = false; static ASimModeBase* SIMMODE; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 300ef22ba..27b271533 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -144,6 +144,11 @@ void ASimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& deb void ASimModeWorldBase::Tick(float DeltaSeconds) { + if (!physics_world_) { + Super::Tick(DeltaSeconds); + return; + } + { //keep this lock as short as possible physics_world_->lock(); diff --git a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp index 1ebd9e592..18d9d9586 100644 --- a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp +++ b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp @@ -12,6 +12,7 @@ #include "Vehicles/Multirotor/MultirotorPawnSimApi.h" #include "Vehicles/Car/SimModeCar.h" #include "Vehicles/ComputerVision/SimModeComputerVision.h" +#include "Vehicles/Rov/SimModeWorldRov.h" #include "common/AirSimSettings.hpp" #include "common/Common.hpp" @@ -962,6 +963,9 @@ void ASimWorldGameMode::CreateSimMode() else if (simmode_name == AirSimSettings::kSimModeTypeComputerVision) SimMode_ = GetWorld()->SpawnActor( FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeRov) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); else { UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, std::string("SimMode is not valid: ") + simmode_name, @@ -1183,9 +1187,10 @@ FString ASimWorldGameMode::GetLaunchPath(const std::string& filename) bool ASimWorldGameMode::GetSettingsText(std::string& settingsText) { return (GetSettingsTextFromCommandLine(settingsText) || - ReadSettingsTextFromFile(FString(msr::airlib::Settings::getExecutableFullPath("settings.json").c_str()), settingsText) || + ReadSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || + ReadSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::getExecutableFullPath("settings.json").c_str()), settingsText) || ReadSettingsTextFromFile(GetLaunchPath("settings.json"), settingsText) || - ReadSettingsTextFromFile(FString(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); + ReadSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); } bool ASimWorldGameMode::GetSettingsTextFromCommandLine(std::string& settingsText) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp new file mode 100644 index 000000000..da1ce4e00 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -0,0 +1,203 @@ +#include "RovPawn.h" +#include "Components/StaticMeshComponent.h" +#include "Components/ChildActorComponent.h" +#include "UObject/ConstructorHelpers.h" +#include "AirBlueprintLib.h" +#include "common/CommonStructs.hpp" +#include "common/Common.hpp" + +ARovPawn::ARovPawn() +{ + init_id_ = pawn_events_.getActuatorSignal().connect_member(this, &ARovPawn::setRotorRenderedStates); + + RootComponent = CreateDefaultSubobject(TEXT("RootComponent")); + + body_mesh_ = CreateDefaultSubobject(TEXT("BodyMesh")); + body_mesh_->SetupAttachment(RootComponent); + + static ConstructorHelpers::FObjectFinder cube_mesh(TEXT("/Engine/BasicShapes/Cube.Cube")); + if (cube_mesh.Succeeded()) { + body_mesh_->SetStaticMesh(cube_mesh.Object); + body_mesh_->SetRelativeScale3D(FVector(0.5f, 0.35f, 0.25f)); + } + + static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); + UClass* pip_class = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + + camera_front_center_comp_ = CreateDefaultSubobject(TEXT("FrontCenterCamera")); + camera_front_center_comp_->SetupAttachment(RootComponent); + camera_front_center_comp_->SetRelativeLocation(FVector(25.f, 0.f, 0.f)); + if (pip_class) camera_front_center_comp_->SetChildActorClass(pip_class); + + camera_front_right_comp_ = CreateDefaultSubobject(TEXT("FrontRightCamera")); + camera_front_right_comp_->SetupAttachment(RootComponent); + camera_front_right_comp_->SetRelativeLocation(FVector(25.f, 15.f, 0.f)); + if (pip_class) camera_front_right_comp_->SetChildActorClass(pip_class); + + camera_front_left_comp_ = CreateDefaultSubobject(TEXT("FrontLeftCamera")); + camera_front_left_comp_->SetupAttachment(RootComponent); + camera_front_left_comp_->SetRelativeLocation(FVector(25.f, -15.f, 0.f)); + if (pip_class) camera_front_left_comp_->SetChildActorClass(pip_class); + + camera_back_center_comp_ = CreateDefaultSubobject(TEXT("BackCenterCamera")); + camera_back_center_comp_->SetupAttachment(RootComponent); + camera_back_center_comp_->SetRelativeLocation(FVector(-25.f, 0.f, 0.f)); + if (pip_class) camera_back_center_comp_->SetChildActorClass(pip_class); + + camera_bottom_center_comp_ = CreateDefaultSubobject(TEXT("BottomCenterCamera")); + camera_bottom_center_comp_->SetupAttachment(RootComponent); + camera_bottom_center_comp_->SetRelativeLocation(FVector(0.f, 0.f, -15.f)); + camera_bottom_center_comp_->SetRelativeRotation(FRotator(-90.f, 0.f, 0.f)); + if (pip_class) camera_bottom_center_comp_->SetChildActorClass(pip_class); +} + +void ARovPawn::BeginPlay() +{ + Super::BeginPlay(); + + if (body_mesh_) { + const TCHAR* candidates[] = { + TEXT("/AirSim/Models/RoV/bluerovheavy2.bluerovheavy2"), + TEXT("/AirSim/Models/RoV/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001"), + TEXT("/Game/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001"), + TEXT("/Game/bluerovheavy2.bluerovheavy2") + }; + UStaticMesh* rov_mesh = nullptr; + for (const TCHAR* candidate : candidates) { + rov_mesh = Cast(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, candidate)); + if (rov_mesh) { + UE_LOG(LogTemp, Log, TEXT("ARovPawn: Loaded ROV mesh from %s"), candidate); + break; + } + } + if (rov_mesh) { + body_mesh_->SetStaticMesh(rov_mesh); + + FBoxSphereBounds bounds = rov_mesh->GetBounds(); + FVector extent = bounds.BoxExtent; + + UE_LOG(LogTemp, Warning, TEXT("ARovPawn ROV Mesh Bounds: Origin=(%f, %f, %f), Extent=(%f, %f, %f)"), bounds.Origin.X, bounds.Origin.Y, bounds.Origin.Z, extent.X, extent.Y, extent.Z); + + float max_dim = FMath::Max3(extent.X, extent.Y, extent.Z) * 2.0f; + float target_size = 50.0f; // BlueROV2 is approx 50cm + float scale = 1.0f; + if (max_dim > 100.0f) { + scale = target_size / max_dim; + } + else if (max_dim < 5.0f && max_dim > 0.001f) { + scale = target_size / max_dim; + } + + body_mesh_->SetRelativeScale3D(FVector(scale, scale, scale)); + body_mesh_->SetRelativeLocation(-bounds.Origin * scale); + } + } + + rotor_speed_components_.Empty(); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV1"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV2"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV3"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV4"))); + + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH1"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH2"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH3"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH4"))); + + rotor_angle_components_.Empty(); + rotor_angle_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("Engine_L"))); + rotor_angle_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("Engine_R"))); +} + +void ARovPawn::initializeForBeginPlay() +{ + //get references of existing camera safely + auto c_fr = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontRightCamera")); + camera_front_right_ = c_fr ? Cast(c_fr->GetChildActor()) : nullptr; + + auto c_fl = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontLeftCamera")); + camera_front_left_ = c_fl ? Cast(c_fl->GetChildActor()) : nullptr; + + auto c_fc = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontCenterCamera")); + camera_front_center_ = c_fc ? Cast(c_fc->GetChildActor()) : nullptr; + + auto c_bc = UAirBlueprintLib::GetActorComponent(this, TEXT("BackCenterCamera")); + camera_back_center_ = c_bc ? Cast(c_bc->GetChildActor()) : nullptr; + + auto c_bot = UAirBlueprintLib::GetActorComponent(this, TEXT("BottomCenterCamera")); + camera_bottom_center_ = c_bot ? Cast(c_bot->GetChildActor()) : nullptr; +} + +void ARovPawn::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + pawn_events_.getPawnTickSignal().emit(DeltaSeconds); +} + +void ARovPawn::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + camera_front_right_ = nullptr; + camera_front_left_ = nullptr; + camera_front_center_ = nullptr; + camera_back_center_ = nullptr; + camera_bottom_center_ = nullptr; + + pawn_events_.getActuatorSignal().disconnect_all(); + rotor_speed_components_.Empty(); + rotor_angle_components_.Empty(); + + Super::EndPlay(EndPlayReason); +} + +const common_utils::UniqueValueMap ARovPawn::getCameras() const +{ + common_utils::UniqueValueMap cameras; + cameras.insert_or_assign("front_center", camera_front_center_); + cameras.insert_or_assign("front_right", camera_front_right_); + cameras.insert_or_assign("front_left", camera_front_left_); + cameras.insert_or_assign("bottom_center", camera_bottom_center_); + cameras.insert_or_assign("back_center", camera_back_center_); + + cameras.insert_or_assign("0", camera_front_center_); + cameras.insert_or_assign("1", camera_front_right_); + cameras.insert_or_assign("2", camera_front_left_); + cameras.insert_or_assign("3", camera_bottom_center_); + cameras.insert_or_assign("4", camera_back_center_); + + cameras.insert_or_assign("", camera_front_center_); + cameras.insert_or_assign("fpv", camera_front_center_); + + return cameras; +} + +void ARovPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, + FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) +{ + pawn_events_.getCollisionSignal().emit(MyComp, Other, OtherComp, bSelfMoved, HitLocation, HitNormal, NormalImpulse, Hit); +} + +void ARovPawn::setRotorRenderedStates(const std::vector& rotor_infos) +{ + int info_count = static_cast(rotor_infos.size()); + int max_rotors = FMath::Min(num_visible_rotors_, info_count); + + for (auto rotor_index = 0; rotor_index < max_rotors; ++rotor_index) { + if (rotor_index < rotor_speed_components_.Num()) { + URotatingMovementComponent* rot_movement = rotor_speed_components_[rotor_index]; + if (rot_movement != nullptr) { + rot_movement->RotationRate.Yaw = + rotor_infos.at(rotor_index).rotor_speed * rotor_infos.at(rotor_index).rotor_direction * RotatorFactor; + } + } + if (!rotor_infos.at(rotor_index).is_fixed) { + if (rotor_index < rotor_angle_components_.Num()) { + UStaticMeshComponent* rotor_angle_comp = rotor_angle_components_[rotor_index]; + if (rotor_angle_comp != nullptr) { + float pitch = -rotor_infos.at(rotor_index).rotor_angle_from_vertical * 180.0f / M_PIf; // negate for unreal axes + FRotator new_rotation = FRotator(pitch, 0.0f, 0.0f); + rotor_angle_comp->SetRelativeRotation(new_rotation); + } + } + } + } +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h new file mode 100644 index 000000000..1ec0f5283 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h @@ -0,0 +1,78 @@ +#pragma once + +#include "GameFramework/RotatingMovementComponent.h" + +#include +#include "PIPCamera.h" +#include "common/common_utils/Signal.hpp" +#include "common/common_utils/UniqueValueMap.hpp" +#include "RovPawnEvents.h" + +#include "RovPawn.generated.h" + +UCLASS() +class AIRSIM_API ARovPawn : public APawn +{ + GENERATED_BODY() + +public: + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Debugging") + float RotatorFactor = 1.3f; + + ARovPawn(); + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, + FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) override; + + //interface + void initializeForBeginPlay(); + const common_utils::UniqueValueMap getCameras() const; + RovPawnEvents* getPawnEvents() + { + return &pawn_events_; + } + //called by API to set rotor speed + void setRotorRenderedStates(const std::vector& rotor_infos); + +private: //variables + UPROPERTY() + class UStaticMeshComponent* body_mesh_; + + UPROPERTY() + class UChildActorComponent* camera_front_right_comp_; + UPROPERTY() + class UChildActorComponent* camera_front_left_comp_; + UPROPERTY() + class UChildActorComponent* camera_front_center_comp_; + UPROPERTY() + class UChildActorComponent* camera_back_center_comp_; + UPROPERTY() + class UChildActorComponent* camera_bottom_center_comp_; + + //Unreal components + UPROPERTY() + APIPCamera* camera_front_left_; + UPROPERTY() + APIPCamera* camera_front_right_; + UPROPERTY() + APIPCamera* camera_front_center_; + UPROPERTY() + APIPCamera* camera_back_center_; + UPROPERTY() + APIPCamera* camera_bottom_center_; + + UPROPERTY() + TArray rotor_speed_components_; + UPROPERTY() + TArray rotor_angle_components_; + + // TODO: hard-coding this value here since using tri-Rov physics with dual-Rov mesh + // back rotor is not animated; order of rotors is 0: left, 1: right, 2: back, so just iterate + // through first two rotors for visualization + const int num_visible_rotors_ = 8; + + RovPawnEvents pawn_events_; + int init_id_; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp new file mode 100644 index 000000000..340206cf3 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp @@ -0,0 +1,6 @@ +#include "RovPawnEvents.h" + +RovPawnEvents::ActuatorsSignal& RovPawnEvents::getActuatorSignal() +{ + return actuator_signal_; +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h new file mode 100644 index 000000000..cf5dae9a0 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h @@ -0,0 +1,31 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Pawn.h" + +#include "PawnEvents.h" +#include "common/Common.hpp" +#include "common/common_utils/Signal.hpp" + +class RovPawnEvents : public PawnEvents +{ +public: //types + typedef msr::airlib::real_T real_T; + struct RotorTiltableInfo + { + real_T rotor_speed = 0; + int rotor_direction = 0; + real_T rotor_thrust = 0; + real_T rotor_control_filtered = 0; + real_T rotor_angle_from_vertical = 0; + bool is_fixed = false; + }; + + typedef common_utils::Signal&> ActuatorsSignal; + +public: + ActuatorsSignal& getActuatorSignal(); + +private: + ActuatorsSignal actuator_signal_; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp new file mode 100644 index 000000000..d6a4268fe --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp @@ -0,0 +1,224 @@ +#include "RovPawnSimApi.h" +#include "AirBlueprintLib.h" +#include "vehicles/rov/RovParamsFactory.hpp" +#include "UnrealSensors/UnrealSensorFactory.h" +#include + +using namespace msr::airlib; + +RovPawnSimApi::RovPawnSimApi(const Params& params) + : PawnSimApi(params), pawn_events_(static_cast(params.pawn_events)) +{ +} + +void RovPawnSimApi::initialize() +{ + PawnSimApi::initialize(); + + //create vehicle API + std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); + vehicle_params_ = RovBodyParamsFactory::createConfig(getVehicleSetting(), sensor_factory); + vehicle_api_ = vehicle_params_->createRovApi(); + //setup physics vehicle + rov_physics_body_ = std::unique_ptr(new RovPhysicsBody(vehicle_params_.get(), vehicle_api_.get(), getKinematics(), getEnvironment())); + + vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment()); + vehicle_api_->setCollisionInfo(CollisionInfo()); + + //initialize private vars + last_phys_pose_ = pending_phys_pose_ = Pose::nanPose(); + pending_pose_status_ = PendingPoseStatus::NonePending; + reset_pending_ = false; + did_reset_ = false; + rotor_count_ = rov_physics_body_->rotorCount(); + rotor_states_.rotors.assign(rotor_count_, RotorTiltableParameters()); + rotor_infos_.assign(rotor_count_, RotorTiltableInfo()); + + //reset roll & pitch of vehicle as aircraft required to be on flat surface at start + Pose pose = getPose(); + float pitch, roll, yaw; + VectorMath::toEulerianAngle(pose.orientation, pitch, roll, yaw); + pose.orientation = VectorMath::toQuaternion(0, 0, yaw); + setPose(pose, false); +} + +void RovPawnSimApi::pawnTick(float dt) +{ + unused(dt); + //calls to update* are handled by physics engine and in SimModeWorldBase +} + +void RovPawnSimApi::updateRenderedState(float dt) +{ + //Utils::log("------Render tick-------"); + + //if reset is pending then do it first, no need to do other things until next tick + if (reset_pending_) { + reset_task_(); + did_reset_ = true; + return; + } + + //move collision info from rendering engine to vehicle + const CollisionInfo& collision_info = getCollisionInfo(); + rov_physics_body_->setCollisionInfo(collision_info); + + last_phys_pose_ = rov_physics_body_->getPose(); + + collision_response = rov_physics_body_->getCollisionResponseInfo(); + + updateRotors(); + + vehicle_api_->getStatusMessages(vehicle_api_messages_); + + if (getRemoteControlID() >= 0) + vehicle_api_->setRCData(getRCData()); + rotor_states_.timestamp = clock()->nowNanos(); + vehicle_api_->setRotorStates(rotor_states_); + vehicle_api_->setCollisionInfo(collision_info); +} + +void RovPawnSimApi::updateRendering(float dt) +{ + //if we did reset then don't worry about synchronizing states for this tick + if (reset_pending_) { + // Continue to wait for reset + if (!did_reset_) { + return; + } + else { + reset_pending_ = false; + did_reset_ = false; + return; + } + } + + if (!VectorMath::hasNan(last_phys_pose_)) { + if (pending_pose_status_ == PendingPoseStatus::RenderPending) { + PawnSimApi::setPose(last_phys_pose_, pending_pose_collisions_); + pending_pose_status_ = PendingPoseStatus::NonePending; + } + else + PawnSimApi::setPose(last_phys_pose_, false); + } + + //UAirBlueprintLib::LogMessage(TEXT("Collision (raw) Count:"), FString::FromInt(collision_response.collision_count_raw), LogDebugLevel::Unimportant); + UAirBlueprintLib::LogMessage(TEXT("Collision Count:"), + FString::FromInt(collision_response.collision_count_non_resting), + LogDebugLevel::Informational); + + for (auto i = 0; i < vehicle_api_messages_.size(); ++i) { + UAirBlueprintLib::LogMessage(FString(vehicle_api_messages_[i].c_str()), TEXT(""), LogDebugLevel::Success, 30); + } + + try { + vehicle_api_->sendTelemetry(dt); + } + catch (std::exception& e) { + UAirBlueprintLib::LogMessage(FString(e.what()), TEXT(""), LogDebugLevel::Failure, 30); + } + + pawn_events_->getActuatorSignal().emit(rotor_infos_); +} + +// update rotor_states_ and rotor_infos_ from aero_physics_body_ +void RovPawnSimApi::updateRotors() +{ + for (uint i = 0; i < rotor_count_; ++i) { + const auto& output = rov_physics_body_->getRotorOutput(i); + // update private rotor variable + rotor_states_.rotors[i].update( + output.rotor_output.thrust, + output.rotor_output.torque_scaler, + output.rotor_output.speed, + output.angle); + // create pointer to rotor_infos_[i] element and update data in-place + RotorTiltableInfo* info = &rotor_infos_[i]; + info->rotor_speed = output.rotor_output.speed; + info->rotor_direction = static_cast(output.rotor_output.turning_direction); + info->rotor_thrust = output.rotor_output.thrust; + info->rotor_control_filtered = output.rotor_output.control_signal_filtered; + info->rotor_angle_from_vertical = output.angle_from_vertical; + info->is_fixed = output.is_fixed; + } +} + +void RovPawnSimApi::setPose(const Pose& pose, bool ignore_collision) +{ + rov_physics_body_->lock(); + rov_physics_body_->setPose(pose); + rov_physics_body_->setGrounded(false); + rov_physics_body_->unlock(); + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +void RovPawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision) +{ + rov_physics_body_->lock(); + rov_physics_body_->updateKinematics(state); + rov_physics_body_->setGrounded(false); + rov_physics_body_->unlock(); + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +void RovPawnSimApi::setPoseCustom(const Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props) +{ + bool correct_num_angles = tilt_angles.size() == rotor_count_; + + rov_physics_body_->lock(); + rov_physics_body_->setPose(pose); + rov_physics_body_->setGrounded(false); + if (correct_num_angles) { + rov_physics_body_->overwriteRotorTilts(tilt_angles, spin_props); + } + updateRotors(); + rov_physics_body_->unlock(); + + // logging done outside of physics lock + if (!correct_num_angles) { + UAirBlueprintLib::LogMessage(TEXT("setPoseCustom called with wrong number of rotor angles: should have "), + FString::FromInt(rotor_count_), + LogDebugLevel::Failure, + 30); + } + + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +//*** Start: UpdatableState implementation ***// +void RovPawnSimApi::resetImplementation() +{ + PawnSimApi::resetImplementation(); + + vehicle_api_->reset(); + rov_physics_body_->reset(); + vehicle_api_messages_.clear(); +} + +//this is high frequency physics tick, flier gets ticked at rendering frame rate +void RovPawnSimApi::update() +{ + //environment update for current position + PawnSimApi::update(); + + //update forces on vertices + rov_physics_body_->update(); + + //update to controller must be done after kinematics have been updated by physics engine +} + +void RovPawnSimApi::reportState(StateReporter& reporter) +{ + PawnSimApi::reportState(reporter); + + rov_physics_body_->reportState(reporter); +} + +RovPawnSimApi::UpdatableObject* RovPawnSimApi::getPhysicsBody() +{ + return rov_physics_body_->getPhysicsBody(); +} +//*** End: UpdatableState implementation ***// diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h new file mode 100644 index 000000000..d8953fba1 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h @@ -0,0 +1,91 @@ +#pragma once + +#include "CoreMinimal.h" + +#include "PawnSimApi.h" +#include "vehicles/rov/RovPhysicsBody.hpp" +#include "vehicles/rov/RovBodyParams.hpp" +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/common_utils/UniqueValueMap.hpp" +#include "RovPawnEvents.h" +#include + +class RovPawnSimApi : public PawnSimApi +{ +public: + typedef msr::airlib::real_T real_T; + typedef msr::airlib::Utils Utils; + typedef msr::airlib::RovPhysicsBody RovPhysicsBody; + typedef msr::airlib::StateReporter StateReporter; + typedef msr::airlib::UpdatableObject UpdatableObject; + typedef msr::airlib::Pose Pose; + + typedef RovPawnEvents::RotorTiltableInfo RotorTiltableInfo; + +public: + virtual void initialize() override; + + virtual ~RovPawnSimApi() = default; + + //VehicleSimApiBase interface + //implements game interface to update pawn + RovPawnSimApi(const Params& params); + virtual void updateRenderedState(float dt) override; + virtual void updateRendering(float dt) override; + + //PhysicsBody interface + //this just wrapped around RovPhysicsBody physics body + virtual void resetImplementation() override; + virtual void update() override; + virtual void reportState(StateReporter& reporter) override; + virtual UpdatableObject* getPhysicsBody() override; + + virtual void setPose(const Pose& pose, bool ignore_collision) override; + virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override; + virtual void pawnTick(float dt) override; + + virtual void setPoseCustom(const Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props) override; + virtual void updateRotors(); + + msr::airlib::RovApiBase* getVehicleApi() const + { + return vehicle_api_.get(); + } + + virtual msr::airlib::VehicleApiBase* getVehicleApiBase() const override + { + return vehicle_api_.get(); + } + +private: + std::unique_ptr vehicle_api_; + std::unique_ptr vehicle_params_; + + std::unique_ptr rov_physics_body_; + unsigned int rotor_count_; + std::vector rotor_infos_; + + //show info on collision response from physics engine + CollisionResponse collision_response; + + RovPawnEvents* pawn_events_; + + //when pose needs to set from non-physics thread, we set it as pending + bool pending_pose_collisions_; + enum class PendingPoseStatus + { + NonePending, + RenderPending + } pending_pose_status_; + Pose pending_phys_pose_; //force new pose through API + + //reset must happen while World is locked so its async task initiated from API thread + bool reset_pending_; + bool did_reset_; + std::packaged_task reset_task_; + + Pose last_phys_pose_; //for trace lines showing vehicle path + std::vector vehicle_api_messages_; + RotorTiltableStates rotor_states_; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp new file mode 100644 index 000000000..7c55f50fc --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp @@ -0,0 +1,167 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#include "SimModeWorldRov.h" +#include "UObject/ConstructorHelpers.h" +#include "Logging/MessageLog.h" +#include "Engine/World.h" +#include "GameFramework/PlayerController.h" + +#include "AirBlueprintLib.h" +#include "vehicles/rov/api/RovApiBase.hpp" +#include "RovPawnSimApi.h" +#include "physics/PhysicsBody.hpp" +#include "common/ClockFactory.hpp" +#include +#include "vehicles/rov/api/RovRpcLibServer.hpp" +#include "common/SteppableClock.hpp" + +void ASimModeWorldRov::BeginPlay() +{ + Super::BeginPlay(); + + //let base class setup physics world + initializeForPlay(); +} + +void ASimModeWorldRov::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + //stop physics thread before we dismantle + stopAsyncUpdator(); + + Super::EndPlay(EndPlayReason); +} + +void ASimModeWorldRov::setupClockSpeed() +{ + typedef msr::airlib::ClockFactory ClockFactory; + + float clock_speed = getSettings().clock_speed; + + //setup clock in ClockFactory + std::string clock_type = getSettings().clock_type; + + if (clock_type == "ScalableClock") { + //scalable clock returns interval same as wall clock but multiplied by a scale factor + ClockFactory::get(std::make_shared(clock_speed == 1 ? 1 : 1 / clock_speed)); + } + else if (clock_type == "SteppableClock") { + //steppable clock returns interval that is a constant number irrespective of wall clock + //we can either multiply this fixed interval by scale factor to speed up/down the clock + //but that would cause vehicles like quadrotors to become unstable + //so alternative we use here is instead to scale control loop frequency. The downside is that + //depending on compute power available, we will max out control loop frequency and therefore can no longer + //get increase in clock speed + + //Approach 1: scale clock period, no longer used now due to quadrotor instability + //ClockFactory::get(std::make_shared( + //static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + + //Approach 2: scale control loop frequency if clock is speeded up + if (clock_speed >= 1) { + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9))); //no clock_speed multiplier + + setPhysicsLoopPeriod(getPhysicsLoopPeriod() / static_cast(clock_speed)); + } + else { + //for slowing down, this don't generate instability + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + } + } + else + throw std::invalid_argument(common_utils::Utils::stringf( + "clock_type %s is not recognized", clock_type.c_str())); +} + +//-------------------------------- overrides -----------------------------------------------// + +APawn* ASimModeWorldRov::createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) +{ + try { + return ASimModeBase::createVehiclePawn(vehicle_setting); + } + catch (const std::exception& e) { + UAirBlueprintLib::LogMessageString("createVehiclePawn fallback to native ARovPawn: ", e.what(), LogDebugLevel::Informational); + + const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); + FVector spawn_position = uu_origin.GetLocation(); + if (!msr::airlib::VectorMath::hasNan(vehicle_setting.position)) + spawn_position = getGlobalNedTransform().fromGlobalNed(vehicle_setting.position); + + FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); + + FActorSpawnParameters pawn_spawn_params; + pawn_spawn_params.Name = FName(vehicle_setting.vehicle_name.c_str()); + pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + APawn* spawned_pawn = this->GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params); + if (spawned_pawn) { + spawned_actors_.Add(spawned_pawn); + return spawned_pawn; + } + throw; + } +} + +std::unique_ptr ASimModeWorldRov::createApiServer() const +{ +#ifdef AIRLIB_NO_RPC + return ASimModeBase::createApiServer(); +#else + UE_LOG(LogTemp, Log, TEXT("SimModeWorldRov: Api server started on port %d"), getSettings().api_port); + return std::unique_ptr(new msr::airlib::RovRpcLibServer( + getApiProvider(), getSettings().api_server_address, getSettings().api_port)); +#endif +} + +void ASimModeWorldRov::getExistingVehiclePawns(TArray& pawns) const +{ + UAirBlueprintLib::FindAllActor(this, pawns); +} + +bool ASimModeWorldRov::isVehicleTypeSupported(const std::string& vehicle_type) const +{ + return ((vehicle_type == AirSimSettings::kVehicleTypeRovSimple) || + (vehicle_type == AirSimSettings::kVehicleTypePX4Rov)); +} + +std::string ASimModeWorldRov::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const +{ + //decide which derived BP to use + std::string pawn_path = vehicle_setting.pawn_path; + if (pawn_path == "") + pawn_path = "DefaultRov"; + + return pawn_path; +} + +PawnEvents* ASimModeWorldRov::getVehiclePawnEvents(APawn* pawn) const +{ + return static_cast(pawn)->getPawnEvents(); +} +const common_utils::UniqueValueMap ASimModeWorldRov::getVehiclePawnCameras( + APawn* pawn) const +{ + return (static_cast(pawn))->getCameras(); +} +void ASimModeWorldRov::initializeVehiclePawn(APawn* pawn) +{ + static_cast(pawn)->initializeForBeginPlay(); +} +std::unique_ptr ASimModeWorldRov::createVehicleSimApi( + const PawnSimApi::Params& pawn_sim_api_params) const +{ + auto vehicle_sim_api = std::unique_ptr(new RovPawnSimApi(pawn_sim_api_params)); + vehicle_sim_api->initialize(); + //For multirotors the vehicle_sim_api are in PhysicsWOrld container and then get reseted when world gets reseted + //vehicle_sim_api->reset(); + return vehicle_sim_api; +} +msr::airlib::VehicleApiBase* ASimModeWorldRov::getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, + const PawnSimApi* sim_api) const +{ + const auto Rov_sim_api = static_cast(sim_api); + return Rov_sim_api->getVehicleApi(); +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h new file mode 100644 index 000000000..c357883c9 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h @@ -0,0 +1,37 @@ +#pragma once + +#include "CoreMinimal.h" + +#include "RovPawn.h" +#include "common/Common.hpp" +#include "SimMode/SimModeWorldBase.h" +#include "api/VehicleSimApiBase.hpp" +#include "SimModeWorldRov.generated.h" + +UCLASS() +class AIRSIM_API ASimModeWorldRov : public ASimModeWorldBase +{ + GENERATED_BODY() + +public: + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + +protected: //overrides + virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) override; + virtual void setupClockSpeed() override; + virtual std::unique_ptr createApiServer() const override; + virtual void getExistingVehiclePawns(TArray& pawns) const override; + virtual bool isVehicleTypeSupported(const std::string& vehicle_type) const override; + virtual std::string getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const override; + virtual PawnEvents* getVehiclePawnEvents(APawn* pawn) const override; + virtual const common_utils::UniqueValueMap getVehiclePawnCameras(APawn* pawn) const override; + virtual void initializeVehiclePawn(APawn* pawn) override; + virtual std::unique_ptr createVehicleSimApi( + const PawnSimApi::Params& pawn_sim_api_params) const override; + virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, + const PawnSimApi* sim_api) const override; + +private: + typedef ARovPawn TVehiclePawn; +}; diff --git a/cmake/AirLib/CMakeLists.txt b/cmake/AirLib/CMakeLists.txt index 6257c7bc4..3a9b7da6c 100644 --- a/cmake/AirLib/CMakeLists.txt +++ b/cmake/AirLib/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5.0) +cmake_minimum_required(VERSION 3.5.0) project(AirLib) LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../cmake-modules") @@ -21,6 +21,8 @@ file(GLOB_RECURSE ${PROJECT_NAME}_sources ${AIRSIM_ROOT}/${PROJECT_NAME}/src/vehicles/car/api/*.cpp ${AIRSIM_ROOT}/${PROJECT_NAME}/src/vehicles/multirotor/*.cpp ${AIRSIM_ROOT}/${PROJECT_NAME}/src/vehicles/multirotor/api/*.cpp + ${AIRSIM_ROOT}/${PROJECT_NAME}/src/vehicles/rov/*.cpp + ${AIRSIM_ROOT}/${PROJECT_NAME}/src/vehicles/rov/api/*.cpp ) add_library(${PROJECT_NAME} STATIC ${${PROJECT_NAME}_sources}) diff --git a/docs/index.md b/docs/index.md index d8f673dca..ac8fda4bc 100644 --- a/docs/index.md +++ b/docs/index.md @@ -1,6 +1,6 @@ # Welcome to AirSim -AirSim is a simulator for drones, cars and more, built on [Unreal Engine](https://www.unrealengine.com/) (we now also have an experimental [Unity](https://unity3d.com/) release). It is open-source, cross platform, and supports software-in-the-loop simulation with popular flight controllers such as PX4 & ArduPilot and hardware-in-loop with PX4 for physically and visually realistic simulations. It is developed as an Unreal plugin that can simply be dropped into any Unreal environment. Similarly, we have an experimental release for a Unity plugin. +AirSim is a simulator for drones, cars, underwater vehicles (ROVs) and more, built on [Unreal Engine](https://www.unrealengine.com/) (we now also have an experimental [Unity](https://unity3d.com/) release). It is open-source, cross platform, and supports software-in-the-loop simulation with popular flight controllers such as PX4 & ArduPilot and hardware-in-loop with PX4 for physically and visually realistic simulations. It is developed as an Unreal plugin that can simply be dropped into any Unreal environment. Similarly, we have an experimental release for a Unity plugin. AirSim was conceived and created by [Shital Shah](https://shital.com) with PX4 and hardware-in-loop code contributed by [Chris Lovett](https://lovettsoftware.com/). @@ -16,6 +16,10 @@ Cars in AirSim [![AirSim Car Demo Video](images/car_demo_video.png)](https://youtu.be/gnz1X3UNM5Y) +ROVs (Underwater Vehicles) in AirSim + +AirSim supports underwater and marine robotics (ROVs / AUVs) with 6-DOF hydrodynamics, fluid buoyancy and drag modeling, and multi-camera perception. See the [ROV Setup and Verification Guide](rov_setup.md) for full configuration, physical parameters, and Python API control examples. + ## How to Get It @@ -51,7 +55,7 @@ View our detailed [English documentation](https://microsoft.github.io/AirSim/)/[ ### Manual drive -If you have remote control (RC) as shown below, you can manually control the drone in the simulator. For cars, you can use arrow keys to drive manually. +If you have remote control (RC) as shown below, you can manually control the drone in the simulator. For cars, you can use arrow keys to drive manually. For underwater vehicles, see [ROV Mode](rov_setup.md). [More details](https://microsoft.github.io/AirSim/remote_control) @@ -66,7 +70,7 @@ AirSim exposes APIs so you can interact with the vehicle in the simulation progr These APIs are also available as part of a separate, independent cross-platform library, so you can deploy them on a companion computer on your vehicle. This way you can write and test your code in the simulator, and later execute it on the real vehicles. Transfer learning and related research is one of our focus areas. -Note that you can use [SimMode setting](https://microsoft.github.io/AirSim/settings#simmode) to specify the default vehicle or the new [ComputerVision mode](https://microsoft.github.io/AirSim/image_apis#computer-vision-mode-1) so you don't get prompted each time you start AirSim. +Note that you can use [SimMode setting](https://microsoft.github.io/AirSim/settings#simmode) to specify the default vehicle (e.g. `Multirotor`, `Car`, or `Rov`) or the new [ComputerVision mode](https://microsoft.github.io/AirSim/image_apis#computer-vision-mode-1) so you don't get prompted each time you start AirSim. For underwater vehicle details, refer to the [ROV Setup and Verification Guide](rov_setup.md). [More details](https://microsoft.github.io/AirSim/apis) diff --git a/docs/rov_setup.md b/docs/rov_setup.md new file mode 100644 index 000000000..5d62b9d26 --- /dev/null +++ b/docs/rov_setup.md @@ -0,0 +1,170 @@ +# Underwater Remotely Operated Vehicle (ROV) Setup and Verification Guide (UE 4.26) + +This document describes how to configure, build, and run the underwater remotely operated vehicle (ROV) simulation in AirSim, and how to verify automated control using the Python API. + +--- + +## 1. Overview and Physics Model + +AirSim provides vehicle support for underwater and marine robotics (ROVs / AUVs), inspired by UNav-Sim dynamics (defaulting to the BlueROV2 Heavy 8-thruster architecture): +* **Hydrodynamics & Physics Model**: Implemented in `AirLib/include/vehicles/rov/`, providing full 6-DOF marine dynamics: + * Fluid buoyancy (computed from water density $\rho = 1028\,\text{kg/m}^3$ and vehicle displacement volume); + * Non-linear fluid drag, quadratic damping, and restoring moments (buoyancy vs. gravity centers); + * 8-thruster spatial force allocation via a thruster mixer matrix. +* **Firmware (`rov_simple`)**: Provides onboard depth holding, attitude leveling, and body-frame velocity control. +* **Unreal Engine Rendering**: Integrated via native C++ class `ARovPawn`, supporting 5 onboard camera mounts (front right, front left, front center, back center, bottom) and animated thruster rotation. + +--- + +## 2. Prerequisites + +* **Operating System**: Windows 10/11 64-bit or Linux (Ubuntu 20.04 / 22.04) +* **Unreal Engine**: Unreal Engine 4.26 +* **Compiler Toolchain**: + * Windows: Visual Studio 2019 (MSVC v142 C++ build tools) + * Linux: GCC 9 / Clang 11+, CMake 3.19+ +* **Python Environment**: Python 3.8 ~ 3.11 (`msgpack-rpc-python`, `numpy`) + +--- + +## 3. Configuration (`settings.json`) + +To run AirSim in ROV mode, configure your `settings.json` accordingly. + +### 3.1 Example Configuration +```json +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", + "SettingsVersion": 1.2, + "SimMode": "Rov", + "ClockSpeed": 1, + "PawnPaths": { + "DefaultQuadrotor": {"PawnBP": "Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'"}, + "DefaultRov": {"PawnBP": "Class'/Script/AirSim.RovPawn'"}, + "DefaultComputerVision": {"PawnBP": "Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'"} + }, + "Vehicles": { + "RovSimple": { + "VehicleType": "RovSimple", + "DefaultVehicleState": "Armed", + "PawnPath": "DefaultRov", + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + }, + "Cameras": { + "front_center_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.50, "Y": 0.00, "Z": 0.00, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + } + } + } + } +} +``` + +### 3.2 Configuration File Locations +AirSim searches for `settings.json` in the following order: +1. Command line argument: `-settings="path/to/settings.json"` +2. Unreal project root directory (e.g. `Unreal/Environments/Blocks/settings.json`) +3. User Documents directory: `~/Documents/AirSim/settings.json` (`C:\Users\\Documents\AirSim\settings.json` on Windows) + +--- + +## 4. Building and Running (Blocks Project) + +Using the built-in `Blocks` project as an example: + +### Step 1: Build the plugin +In the `air` root directory: +* **Windows**: + ```cmd + build.cmd + ``` +* Sync the plugin to the `Blocks` environment: + ```cmd + cd Unreal\Environments\Blocks + update_from_git.bat + ``` + +### Step 2: Launch the simulation +* Open `Unreal/Environments/Blocks/Blocks.uproject` in Unreal Engine 4.26; +* Click the **Play** button in the editor toolbar; +* The ROV will spawn in the viewport. The log outputs: + ```text + LogTemp: StartupModule: AirSim plugin + LogTemp: ARovPawn: Loaded ROV mesh from /AirSim/Models/RoV/... + LogTemp: RovSimple + LogTemp: SimModeWorldRov: Api server started on port 41451 + ``` + +--- + +## 5. Python API Verification + +While the simulation is running: + +### Step 1: Install Python client +```bash +cd PythonClient +pip install -e . +``` + +### Step 2: Run verification script +```bash +python PythonClient/rov/hello_rov.py +``` + +### Step 3: Expected output +```text +Connecting to AirSim ROV... +Connected! +Client Ver:1 (Min Req: 1) +Server Ver:1 (Min Req: 1) + +Arming the ROV... + +ROV State: + Kinematics Position: x=0.00, y=0.00, z=0.00 + Kinematics Orientation: w=1.00, x=0.00, y=0.00, z=0.00 + Linear Velocity: vx=0.00, vy=0.00, vz=0.00 + +IMU Data: + Linear Acceleration: (0.00, 0.00, -9.81) m/s^2 + Angular Velocity: (0.00, 0.00, 0.00) rad/s + +Barometer (Depth) Data: + Altitude: 0.00 m + Pressure: 101325.0 Pa + +Magnetometer Data: + Magnetic Field: (0.21, 0.02, 0.42) Gauss + +Moving ROV by velocity in body frame (forward 0.5 m/s, dive 0.2 m/s for 3s)... +Updated ROV Position: x=1.48, y=0.00, z=0.59 + +Hovering... +Disarming and resetting... +Done! +``` + +--- + +## 6. Troubleshooting + +1. **Connection refused on port 41451**: + - Ensure the Unreal simulation is actively running (Play mode). + - Check that `"SimMode": "Rov"` is specified in `settings.json`. +2. **Vehicle not spawning or mesh missing**: + - Verify that `PawnPaths` in `settings.json` contains `"DefaultRov": {"PawnBP": "Class'/Script/AirSim.RovPawn'"}`. diff --git a/mkdocs.yml b/mkdocs.yml index fa34d940e..7c7c0f778 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -60,6 +60,7 @@ nav: - "Settings": 'settings.md' - "Camera Views": 'camera_views.md' - "Car Mode": 'using_car.md' + - "ROV Mode": 'rov_setup.md' - "Remote Control": 'remote_control.md' - "XBox Controller": 'xbox_controller.md' - "Steering Wheel": 'steering_wheel_installation.md'