diff --git a/.gitignore b/.gitignore
index feff8336d1..a693ce8c47 100644
--- a/.gitignore
+++ b/.gitignore
@@ -331,6 +331,7 @@ cmake_build/
/3.3.2.zip
/PythonClient/**/cloud.txt
/PythonClient/**/cloud.asc
+/Unreal/Environments/BlocksV2/BlocksV2.sln
/Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/
/suv_download_tmp
car_assets.zip
diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj
index 5bc8b7775d..71a5acb3e7 100644
--- a/AirLib/AirLib.vcxproj
+++ b/AirLib/AirLib.vcxproj
@@ -96,6 +96,8 @@
+
+
diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters
index 885766832d..045e337dfd 100644
--- a/AirLib/AirLib.vcxproj.filters
+++ b/AirLib/AirLib.vcxproj.filters
@@ -456,6 +456,12 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index f06701f9a1..3cd501d17e 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -32,6 +32,7 @@ namespace airlib
static constexpr char const* kVehicleTypeArduCopterSolo = "arducoptersolo";
static constexpr char const* kVehicleTypeSimpleFlight = "simpleflight";
static constexpr char const* kVehicleTypeArduCopter = "arducopter";
+ static constexpr char const* kVehicleTypeBetaflight = "betaflight";
static constexpr char const* kVehicleTypePhysXCar = "physxcar";
static constexpr char const* kVehicleTypeArduRover = "ardurover";
static constexpr char const* kVehicleTypeComputerVision = "computervision";
@@ -817,7 +818,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 == kVehicleTypeBetaflight || vehicle_type == kVehicleTypeArduRover)
vehicle_setting = createMavLinkVehicleSetting(settings_json);
//for everything else we don't need derived class yet
else {
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
index 80a840ed07..44916dd4e6 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp
@@ -9,6 +9,7 @@
#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp"
#include "vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp"
#include "vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp"
+#include "vehicles/multirotor/firmwares/betaflight/BetaflightParams.hpp"
namespace msr
{
@@ -33,6 +34,9 @@ namespace airlib
else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduCopter) {
config.reset(new ArduCopterParams(*static_cast(vehicle_setting), sensor_factory));
}
+ else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeBetaflight) {
+ config.reset(new BetaflightParams(*static_cast(vehicle_setting), sensor_factory));
+ }
else if (vehicle_setting->vehicle_type == "" || //default config
vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) {
config.reset(new SimpleFlightQuadXParams(vehicle_setting, sensor_factory));
diff --git a/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightApi.hpp
new file mode 100644
index 0000000000..19bf6224ba
--- /dev/null
+++ b/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightApi.hpp
@@ -0,0 +1,484 @@
+#ifndef msr_airlib_BetaflightDroneController_hpp
+#define msr_airlib_BetaflightDroneController_hpp
+
+#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
+#include "sensors/SensorCollection.hpp"
+#include "physics/Environment.hpp"
+#include "physics/Kinematics.hpp"
+#include "vehicles/multirotor/MultiRotorParams.hpp"
+#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp"
+#include "common/Common.hpp"
+#include "physics/PhysicsBody.hpp"
+#include "common/AirSimSettings.hpp"
+
+// Sensors
+#include "sensors/imu/ImuBase.hpp"
+#include "sensors/barometer/BarometerBase.hpp"
+
+#include "UdpSocket.hpp"
+
+namespace msr
+{
+namespace airlib
+{
+
+ class BetaflightApi : public MultirotorApiBase
+ {
+
+ public:
+ BetaflightApi(const MultiRotorParams* vehicle_params, const AirSimSettings::MavLinkConnectionInfo& connection_info)
+ : connection_info_(connection_info), vehicle_params_(vehicle_params)
+ {
+ sensors_ = &getSensors();
+ start_time_ = ClockFactory::get()->nowNanos();
+ resetRotors();
+ connect();
+ }
+
+ ~BetaflightApi()
+ {
+ closeConnections();
+ }
+
+ public:
+ virtual void resetImplementation() override
+ {
+ MultirotorApiBase::resetImplementation();
+
+ // Reset state
+ }
+
+ // Update sensor data & send to betaflight
+ virtual void update() override
+ {
+ MultirotorApiBase::update();
+
+ // Time
+ double timestamp = static_cast(ClockFactory::get()->nowNanos() - start_time_) / 1E9;
+
+ if (timestamp >= start_timestamp) {
+
+ if (failed_pkts == 0) sendState(timestamp);
+
+ if (recvControl()) {
+
+ failed_pkts = 0;
+ if (!is_connected_) is_connected_ = true;
+ }
+ else {
+
+ failed_pkts++;
+
+ if (failed_pkts >= max_failed_pkts) {
+
+ failed_pkts = 0;
+ is_connected_ = false;
+ start_timestamp = timestamp + wait_time;
+ resetRotors();
+ Utils::log(Utils::stringf("Rotors data not recieved. Retry in %.0f sec", wait_time), Utils::kLogLevelWarn);
+ }
+ }
+ }
+ }
+
+ // necessary overrides
+ virtual bool isApiControlEnabled() const override
+ {
+ return true;
+ }
+ virtual void enableApiControl(bool is_enabled) override
+ {
+ // Utils::log("Not Implemented: enableApiControl", Utils::kLogLevelInfo);
+ unused(is_enabled);
+ }
+ virtual bool armDisarm(bool arm) override
+ {
+ unused(arm);
+ return is_connected_;
+ }
+ virtual GeoPoint getHomeGeoPoint() const override
+ {
+ return physics_->getEnvironment().getHomeGeoPoint();
+ }
+ virtual void getStatusMessages(std::vector& messages) override
+ {
+ unused(messages);
+ }
+
+ virtual const SensorCollection& getSensors() const override
+ {
+ return vehicle_params_->getSensors();
+ }
+
+ public: //TODO:MultirotorApiBase implementation
+ virtual real_T getActuation(unsigned int rotor_index) const override
+ {
+ return rotor_controls_[rotor_index];
+ }
+
+ virtual size_t getActuatorCount() const override
+ {
+ return vehicle_params_->getParams().rotor_count;
+ }
+
+ virtual void moveByRC(const RCData& rc_data) override
+ {
+ unused(rc_data);
+ // setRCData(rc_data);
+ }
+
+ virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override
+ {
+ //Utils::log("Not Implemented: setSimulatedGroundTruth", Utils::kLogLevelInfo);
+ unused(kinematics);
+ unused(environment);
+
+ if (physics_ == nullptr) {
+ physics_ = static_cast(this->getParent());
+ }
+ }
+
+ virtual bool setRCData(const RCData& rc_data) override
+ {
+ last_rcData_ = rc_data;
+ is_rc_connected_ = true;
+
+ return true;
+ }
+
+ protected:
+ virtual Kinematics::State getKinematicsEstimated() const override
+ {
+ return physics_->getKinematics();
+ }
+
+ virtual LandedState getLandedState() const override
+ {
+ return physics_->isGrounded() ? 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 physics_->getEnvironment().getState().geo_point;
+ }
+
+ 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;
+ return 3.0;
+ }
+
+ 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 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(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
+ {
+ unused(front_right_pwm);
+ unused(front_left_pwm);
+ unused(rear_right_pwm);
+ unused(rear_left_pwm);
+ Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
+ }
+
+ virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
+ {
+ unused(roll);
+ unused(pitch);
+ unused(yaw_rate);
+ unused(throttle);
+ Utils::log("Not Implemented: commandRollPitchYawrateThrottle", Utils::kLogLevelInfo);
+ }
+
+ virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
+ {
+ 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
+ {
+ unused(roll);
+ unused(pitch);
+ unused(yaw);
+ unused(throttle);
+ Utils::log("Not Implemented: commandRollPitchYawThrottle", Utils::kLogLevelInfo);
+ }
+
+ virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
+ {
+ 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
+ {
+ 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
+ {
+ unused(roll_rate);
+ unused(pitch_rate);
+ unused(yaw_rate);
+ unused(throttle);
+ Utils::log("Not Implemented: commandAngleRatesZ", Utils::kLogLevelInfo);
+ }
+
+ virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
+ {
+ 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
+ {
+ unused(vx);
+ unused(vy);
+ unused(z);
+ unused(yaw_mode);
+ Utils::log("Not Implemented: commandVelocityZ", Utils::kLogLevelInfo);
+ }
+
+ virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
+ {
+ unused(x);
+ unused(y);
+ unused(z);
+ unused(yaw_mode);
+ Utils::log("Not Implemented: commandPosition", Utils::kLogLevelInfo);
+ }
+
+ virtual const MultirotorApiParams& getMultirotorApiParams() const override
+ {
+ return safety_params_;
+ }
+
+ protected:
+ void closeConnections()
+ {
+ if (motor_socket_ != nullptr)
+ motor_socket_->close();
+
+ if (servo_socket_ != nullptr)
+ servo_socket_->close();
+ }
+
+ void connect()
+ {
+ ip_ = connection_info_.udp_address;
+
+ closeConnections();
+
+ if (ip_ == "") {
+ throw std::invalid_argument("UdpIp setting is invalid.");
+ }
+
+ Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for receiving servo position", servo_port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+ Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for receiving rotor power", motor_port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+ Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", state_port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+
+ motor_socket_ = std::make_unique();
+ motor_socket_->bind(connection_info_.local_host_ip, motor_port_);
+
+ // servo_socket_ = std::make_unique();
+ // servo_socket_->bind(connection_info_.local_host_ip, servo_port_);
+ }
+
+ void resetRotors()
+ {
+ for (auto i = 0; i < kBetaflightMotorCount; i++)
+ rotor_controls_[i] = 0;
+ }
+
+ private:
+ // actual functions
+
+ void sendState(double timestamp) // send fdmPacket to betaflight i.e udp:9002
+ {
+ if (sensors_ == nullptr || motor_socket_ == nullptr)
+ return;
+
+ FdmPacket pkt = FdmPacket();
+
+ // IMU
+ const auto& imu_output = getImuData("");
+
+ // Baro
+ const auto& baro_output = getBarometerData("");
+
+ // Time
+ pkt.timestamp = timestamp;
+
+ // Angular Velocity
+ pkt.imu_angular_velocity_rpy[0] = imu_output.angular_velocity[0];
+ pkt.imu_angular_velocity_rpy[1] = imu_output.angular_velocity[1];
+ pkt.imu_angular_velocity_rpy[2] = imu_output.angular_velocity[2];
+
+ // Linear Acceleration
+ pkt.imu_linear_acceleration_xyz[0] = imu_output.linear_acceleration[0];
+ pkt.imu_linear_acceleration_xyz[1] = imu_output.linear_acceleration[1];
+ pkt.imu_linear_acceleration_xyz[2] = imu_output.linear_acceleration[2];
+
+ // Orientation Quaternion. In case USE_IMU_CALC is not defined on betaflight side
+ pkt.imu_orientation_quat[0] = imu_output.orientation.w();
+ pkt.imu_orientation_quat[1] = imu_output.orientation.x();
+ pkt.imu_orientation_quat[2] = imu_output.orientation.y();
+ pkt.imu_orientation_quat[3] = imu_output.orientation.z();
+
+ // Position
+ pkt.position_xyz[0] = 0;
+ pkt.position_xyz[1] = 0;
+ pkt.position_xyz[2] = 0;
+
+ // Velocity
+ pkt.velocity_xyz[0] = 0;
+ pkt.velocity_xyz[1] = 0;
+ pkt.velocity_xyz[2] = 0;
+
+ // Pressure
+ pkt.pressure = baro_output.pressure;
+
+ motor_socket_->sendto(&pkt, sizeof(pkt), ip_, state_port_);
+ }
+
+ bool recvControl()
+ {
+ ServoPacket s_pkt;
+ int s_recv_ret = motor_socket_->recv(&s_pkt, sizeof(s_pkt), 100);
+
+ if (s_recv_ret != sizeof(s_pkt)) {
+ if (s_recv_ret <= 0) {
+ Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", s_recv_ret), Utils::kLogLevelInfo);
+ }
+ else {
+ Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes for rotors", s_recv_ret, sizeof(s_pkt)), Utils::kLogLevelInfo);
+ }
+
+ return false;
+ }
+
+ for (auto i = 0; i < kBetaflightMotorCount; ++i) {
+ rotor_controls_[i] = s_pkt.pwm[i];
+ }
+
+ // ServoPacketRaw sr_pkt;
+ // int sr_recv_ret = servo_socket_->recv(&sr_pkt, sizeof(sr_pkt), 100);
+
+ // if (sr_recv_ret != sizeof(sr_pkt)) {
+ // if (sr_recv_ret <= 0) {
+ // Utils::log(Utils::stringf("Error while receiving servo control data - ErrorNo: %d", sr_recv_ret), Utils::kLogLevelInfo);
+ // }
+ // else {
+ // Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes for servos", sr_recv_ret, sizeof(sr_pkt)), Utils::kLogLevelInfo);
+ // }
+ // }
+ // else {
+
+ // for (auto i = 0; i < kBetaflightServoCount; ++i) {
+ // servo_controls_[i] = sr_pkt.pwm[i];
+ // }
+ // }
+
+ // Utils::log(Utils::stringf("pwm received: [ %f, %f, %f, %f ]", rotor_controls_[0], rotor_controls_[1], rotor_controls_[2], rotor_controls_[3]), Utils::kLogLevelInfo);
+ return true;
+ }
+
+ private:
+ struct FdmPacket // equivalent of fdm_packet in betaflight SITL
+ {
+ double timestamp; // in seconds
+ double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se
+ double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256
+ double imu_orientation_quat[4]; //w, x, y, z
+ double velocity_xyz[3]; // m/s, earth frame
+ double position_xyz[3]; // meters, NED from origin
+ double pressure; // pascals, atmosphere pressure
+ };
+
+ static const int kBetaflightMotorCount = 4;
+ static const int kBetaflightServoCount = 8;
+
+ static const int kBetaflightMaxRCCount = 16;
+
+ struct ServoPacket // equivalent of servo_packet in betaflight SITL
+ {
+ float pwm[kBetaflightMotorCount];
+ };
+
+ struct ServoPacketRaw // equivalent of servo_packet_raw in betaflight SITL
+ {
+ uint16_t motorCount; // Count of motor in the PWM output.
+ float pwm[kBetaflightServoCount]; // Raw PWM from 1100 to 1900
+ };
+
+ float rotor_controls_[kBetaflightMotorCount];
+ float servo_controls_[kBetaflightServoCount];
+
+ // Sockets
+ std::unique_ptr motor_socket_;
+ std::unique_ptr servo_socket_;
+
+ const uint16_t servo_port_ = 9001;
+ const uint16_t motor_port_ = 9002;
+ const uint16_t state_port_ = 9003;
+ const uint16_t rc_port_ = 9004;
+
+ MultirotorApiParams safety_params_;
+ AirSimSettings::MavLinkConnectionInfo connection_info_;
+ const MultiRotorParams* vehicle_params_;
+ const MultiRotorPhysicsBody* physics_ = nullptr;
+ const SensorCollection* sensors_;
+
+ std::string ip_;
+ RCData last_rcData_;
+ bool is_connected_ = false;
+ bool is_rc_connected_;
+ uint64_t start_time_; // changed in contructor call
+
+ uint8_t failed_pkts = 0;
+ const uint8_t max_failed_pkts = 5; // maximum amount of failed packets
+
+ double start_timestamp = 0;
+ double wait_time = 5;
+ };
+}
+} // namespace
+
+#endif
\ No newline at end of file
diff --git a/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightParams.hpp
new file mode 100644
index 0000000000..73eeed3b9e
--- /dev/null
+++ b/AirLib/include/vehicles/multirotor/firmwares/betaflight/BetaflightParams.hpp
@@ -0,0 +1,60 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef msr_airlib_vehicles_BetaflightParams_hpp
+#define msr_airlib_vehicles_BetaflightParams_hpp
+
+#include "vehicles/multirotor/firmwares/betaflight/BetaflightApi.hpp"
+#include "vehicles/multirotor/MultiRotorParams.hpp"
+#include "common/AirSimSettings.hpp"
+#include "sensors/SensorFactory.hpp"
+
+namespace msr
+{
+namespace airlib
+{
+
+ class BetaflightParams : public MultiRotorParams
+ {
+ public:
+ BetaflightParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting, std::shared_ptr sensor_factory)
+ : sensor_factory_(sensor_factory)
+ {
+ connection_info_ = getConnectionInfo(vehicle_setting);
+ }
+
+ virtual ~BetaflightParams() = default;
+
+ virtual std::unique_ptr createMultirotorApi() override
+ {
+ return std::unique_ptr(new BetaflightApi(this, connection_info_));
+ }
+
+ protected:
+ virtual void setupParams() override
+ {
+ auto& params = getParams();
+
+ // Use connection_info_.model for the model name, see Px4MultiRotorParams for example
+
+ // Only Generic for now
+ setupFrameGenericQuad(params);
+ }
+
+ virtual const SensorFactory* getSensorFactory() const override
+ {
+ return sensor_factory_.get();
+ }
+
+ 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
\ No newline at end of file
diff --git a/Betaflight/BTFL_quadrocopter.txt b/Betaflight/BTFL_quadrocopter.txt
new file mode 100644
index 0000000000..d8014893ec
--- /dev/null
+++ b/Betaflight/BTFL_quadrocopter.txt
@@ -0,0 +1,47 @@
+
+# start the command batch
+batch start
+
+board_name SITL
+
+# feature
+feature -GPS
+
+# aux
+aux 0 0 0 1700 2100 0 0
+aux 1 1 1 1700 2100 0 0
+
+# master
+set motor_pwm_protocol = PWM
+set pid_process_denom = 16
+
+profile 0
+
+# profile 0
+set p_pitch = 61
+set i_pitch = 109
+set d_pitch = 59
+set f_pitch = 81
+set p_roll = 58
+set i_roll = 104
+set d_roll = 52
+set f_roll = 78
+set p_yaw = 58
+set i_yaw = 104
+set f_yaw = 78
+set d_min_roll = 39
+set d_min_pitch = 44
+set simplified_master_multiplier = 130
+set simplified_feedforward_gain = 50
+
+rateprofile 0
+
+# rateprofile 0
+set roll_srate = 33
+set pitch_srate = 33
+set yaw_srate = 33
+
+# end the command batch
+batch end
+
+save
\ No newline at end of file
diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py
index 961bceb244..398e022862 100644
--- a/PythonClient/airsim/types.py
+++ b/PythonClient/airsim/types.py
@@ -31,7 +31,9 @@ def from_msgpack(cls, encoded):
for index, (attr_name, attr_type) in enumerate(cls.attribute_order):
value = encoded[index]
- if issubclass(attr_type, MsgpackMixin):
+ if isinstance(value, list) and value and isinstance(value[0], MsgpackMixin):
+ value = [v.from_msgpack() for v in value]
+ elif issubclass(attr_type, MsgpackMixin):
value = attr_type.from_msgpack(value)
setattr(obj, attr_name, value)
@@ -555,7 +557,6 @@ class CarState(MsgpackMixin):
rpm = 0.0
maxrpm = 0.0
handbrake = False
- collision = CollisionInfo()
kinematics_estimated = KinematicsState()
timestamp = np.uint64(0)
@@ -565,7 +566,6 @@ class CarState(MsgpackMixin):
('rpm', float),
('maxrpm', float),
('handbrake', bool),
- ('collision', CollisionInfo),
('kinematics_estimated', KinematicsState),
('timestamp', np.uint64)
]
@@ -588,20 +588,29 @@ class MultirotorState(MsgpackMixin):
('gps_location', GeoPoint),
('timestamp', np.uint64),
('landed_state', int),
- ('rc_data', RCData),
- ('ready', bool),
- ('ready_message', str),
- ('can_arm', bool)
+ ('rc_data', RCData)
+ ]
+
+
+class RotorParameters(MsgpackMixin):
+ thrust = 0.0
+ torque_scaler = 0.0
+ speed = 0.0
+
+ attribute_order = [
+ ('thrust', float),
+ ('torque_scaler', float),
+ ('speed', float)
]
class RotorStates(MsgpackMixin):
timestamp = np.uint64(0)
- rotors = []
+ rotors: list[RotorParameters] = []
attribute_order = [
- ('timestamp', np.uint64),
- ('rotors', list)
+ ('rotors', list[RotorParameters]),
+ ('timestamp', np.uint64)
]
diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/BP_CameraDirector.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/BP_CameraDirector.uasset
index bf8771b881..577097c4b9 100644
Binary files a/Unreal/Plugins/AirSim/Content/Blueprints/BP_CameraDirector.uasset and b/Unreal/Plugins/AirSim/Content/Blueprints/BP_CameraDirector.uasset differ
diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
index 7f6f745779..3931456d63 100644
--- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
+++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
@@ -51,7 +51,7 @@ EAppReturnType::Type UAirBlueprintLib::ShowMessage(EAppMsgType::Type message_typ
return FMessageDialog::Open(message_type,
FText::FromString(message.c_str()),
- title_text);
+ &title_text);
}
void UAirBlueprintLib::enableWorldRendering(AActor* context, bool enable)
diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs
index de9041afe0..11397360fa 100644
--- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs
+++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs
@@ -91,8 +91,13 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target)
PublicIncludePaths.Add(Path.Combine(AirLibPath, "include"));
PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", "eigen3"));
AddOSLibDependencies(Target);
+
+ CompileMode cmplMode = CompileMode.CppCompileWithRpc;
+
+ if (Target.Platform == UnrealTargetPlatform.Linux)
+ cmplMode = CompileMode.HeaderOnlyWithRpc;
- SetupCompileMode(CompileMode.CppCompileWithRpc, Target);
+ SetupCompileMode(cmplMode, Target);
}
private void AddOSLibDependencies(ReadOnlyTargetRules Target)
diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp
index 86cf0317ad..d737dc26be 100644
--- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp
+++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp
@@ -94,10 +94,12 @@ void ASimModeWorldMultiRotor::getExistingVehiclePawns(TArray& pawns) co
bool ASimModeWorldMultiRotor::isVehicleTypeSupported(const std::string& vehicle_type) const
{
- return ((vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) ||
- (vehicle_type == AirSimSettings::kVehicleTypePX4) ||
- (vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo) ||
- (vehicle_type == AirSimSettings::kVehicleTypeArduCopter));
+ return ((vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight)
+ || (vehicle_type == AirSimSettings::kVehicleTypePX4)
+ || (vehicle_type == AirSimSettings::kVehicleTypeArduCopterSolo)
+ || (vehicle_type == AirSimSettings::kVehicleTypeBetaflight)
+ || (vehicle_type == AirSimSettings::kVehicleTypeArduCopter)
+ );
}
std::string ASimModeWorldMultiRotor::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const
diff --git a/UnrealPluginFiles.vcxproj b/UnrealPluginFiles.vcxproj
index af6646ccdb..9c9f3f2d68 100644
--- a/UnrealPluginFiles.vcxproj
+++ b/UnrealPluginFiles.vcxproj
@@ -98,73 +98,74 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
+
-
diff --git a/setup.sh b/setup.sh
index 47cc03e81b..9bb41adf26 100755
--- a/setup.sh
+++ b/setup.sh
@@ -49,8 +49,7 @@ else #linux
rsync \
software-properties-common \
wget \
- libvulkan1 \
- vulkan-utils
+ libvulkan1
#install clang and build tools
VERSION=$(lsb_release -rs | cut -d. -f1)
@@ -61,6 +60,13 @@ else #linux
sudo apt-get update
fi
sudo apt-get install -y clang-12 clang++-12 libc++-12-dev libc++abi-12-dev
+
+
+ if [ "$VERSION" -gt "20" ]; then
+ sudo apt-get -y install --no-install-recommends vulkan-tools libunwind-dev
+ else
+ sudo apt-get -y install --no-install-recommends vulkan-utils
+ fi
fi
if ! which cmake; then