ArduCopterSolo support

AirLib/AirLib.vcxproj

@@ -86,6 +86,8 @@
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibAdapators.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibClient.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibServer.hpp" />
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloApi.hpp" />
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloParams.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightBoard.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightCommLink.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightEstimator.hpp" />
@@ -170,6 +172,7 @@
   <ItemGroup>
     <ClCompile Include="src\api\RpcLibClientBase.cpp" />
     <ClCompile Include="src\api\RpcLibServerBase.cpp" />
+    <ClCompile Include="src\vehicles\multirotor\MultiRotorParamsFactory.cpp" />
     <ClCompile Include="src\vehicles\multirotor\api\MultirotorApiBase.cpp" />
     <ClCompile Include="src\safety\ObstacleMap.cpp" />
     <ClCompile Include="src\safety\SafetyEval.cpp" />

AirLib/AirLib.vcxproj.filters

@@ -447,6 +447,12 @@
     <ClInclude Include="include\common\common_utils\ColorUtils.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloApi.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloParams.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">
@@ -479,5 +485,11 @@
     <ClCompile Include="src\vehicles\multirotor\api\MultirotorApiBase.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="src\vehicles\multirotor\MultiRotorParamsFactory.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="src\vehicles\MultiRotorParamsFactory.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>

AirLib/include/common/AirSimSettings.hpp

@@ -23,7 +23,8 @@ struct AirSimSettings {
 public: //types
     static constexpr int kSubwindowCount = 3; //must be >= 3 for now
     static constexpr char const * kVehicleTypePX4 = "px4multirotor";
-    static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
+	static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo";
+	static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
     static constexpr char const * kVehicleTypePhysXCar = "physxcar";
     static constexpr char const * kVehicleTypeComputerVision = "computervision";
 
@@ -242,13 +243,9 @@ struct AirSimSettings {
         std::string model = "Generic";
     };
 
-    struct PX4VehicleSetting : public VehicleSetting {
-        MavLinkConnectionInfo connection_info;
-    };
-
-    struct SimpleFlightVehicleSetting : public VehicleSetting {
-        MavLinkConnectionInfo connection_info;
-    };
+	struct MavLinkVehicleSetting : public VehicleSetting {
+		MavLinkConnectionInfo connection_info;
+	};
 
     struct SegmentationSetting {
         enum class InitMethodType {
@@ -570,11 +567,11 @@ struct AirSimSettings {
         }
     }
 
-    static std::unique_ptr<VehicleSetting> createPX4VehicleSetting(const Settings& settings_json) 
+    static std::unique_ptr<VehicleSetting> createMavLinkVehicleSetting(const Settings& settings_json) 
     {
         //these settings_json are expected in same section, not in another child
-        std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new PX4VehicleSetting());
-        PX4VehicleSetting* vehicle_setting = static_cast<PX4VehicleSetting*>(vehicle_setting_p.get());
+        std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new MavLinkVehicleSetting());
+		MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
 
         //TODO: we should be selecting remote if available else keyboard
         //currently keyboard is not supported so use rc as default
@@ -632,8 +629,8 @@ struct AirSimSettings {
         auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
         std::unique_ptr<VehicleSetting> vehicle_setting;
-        if (vehicle_type == kVehicleTypePX4)
-            vehicle_setting = createPX4VehicleSetting(settings_json);
+        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo)
+            vehicle_setting = createMavLinkVehicleSetting(settings_json);
         //for everything else we don't need derived class yet
         else {
             vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());

AirLib/include/common/CommonStructs.hpp

@@ -166,7 +166,7 @@ struct GeoPoint {
         return os << "[" << g.latitude << ", " << g.longitude << ", " << g.altitude << "]";
     }
 
-    std::string to_string()
+    std::string to_string() const
     {
         return std::to_string(latitude) + string(", ") + std::to_string(longitude) + string(", ") + std::to_string(altitude);
     }

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -100,7 +100,15 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         body.setKinematics(next);
         body.setWrench(next_wrench);
         body.kinematicsUpdated();
-    }
+
+
+		////// (FIXME this is from PhysicsBody, where it's commented out - it appears that we need it here to ensure that GPS sensing functions properly)
+		//TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
+		body.getEnvironment().setPosition(next.pose.position);
+		body.getEnvironment().update();
+		////// (End FIXME)
+
+	}
 
     static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, 
         bool is_collision_response, CollisionResponse& collision_response)

AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp

@@ -4,35 +4,26 @@
 #ifndef msr_airlib_vehicles_MultiRotorParamsFactory_hpp
 #define msr_airlib_vehicles_MultiRotorParamsFactory_hpp
 
-#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"
-#include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp"
-#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp"
+#include "vehicles/multirotor/MultiRotorParams.hpp"
+#include "common/AirSimSettings.hpp"
+#include "sensors/SensorFactory.hpp"
 
 
 namespace msr { namespace airlib {
 
 class MultiRotorParamsFactory {
 public:
-    static std::unique_ptr<MultiRotorParams> createConfig(const AirSimSettings::VehicleSetting* vehicle_setting, 
-        std::shared_ptr<const SensorFactory> sensor_factory)
-    {
-        std::unique_ptr<MultiRotorParams> config;
-
-        if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4) {
-            config.reset(new Px4MultiRotorParams(* static_cast<const AirSimSettings::PX4VehicleSetting*>(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));
-        } 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();
-
-        return config;
-    }
+
+	static void reset();
+
+	static std::unique_ptr<MultiRotorParams> createConfig(const AirSimSettings::VehicleSetting* vehicle_setting,
+		std::shared_ptr<const SensorFactory> sensor_factory);
+
+private:
+
+	// Simple zero-based ID for ArduCopterSolo vehicles
+	static int next_solo_id_;
+
 };
 
 }} //namespace

AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp

@@ -0,0 +1,221 @@
+#ifndef msr_airlib_ArduCopterSoloApi_h
+#define msr_airlib_ArduCopterSoloApi_h
+
+#include "AdHocConnection.hpp"
+#include "vehicles/multirotor/MultiRotor.hpp"
+#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"
+
+namespace msr { namespace airlib {
+
+
+class ArduCopterSoloApi : public MavLinkMultirotorApi
+{
+public:
+	virtual ~ArduCopterSoloApi()
+	{
+		closeAllConnection();
+	}
+
+	virtual void update()
+	{
+		if (sensors_ == nullptr)
+			return;
+
+		// send GPS and other sensor updates
+		const auto gps = getGps();
+		if (gps != nullptr) {
+			const auto& gps_output = gps->getOutput();
+			const auto& imu_output = getImu()->getOutput();
+			//                    const auto& mag_output = getMagnetometer()->getOutput();
+			//                    const auto& baro_output = getBarometer()->getOutput();
+
+			SensorMessage packet;
+			packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000;
+			packet.latitude = gps_output.gnss.geo_point.latitude;
+			packet.longitude = gps_output.gnss.geo_point.longitude;
+			packet.altitude = gps_output.gnss.geo_point.altitude;
+
+			common_utils::Utils::log("Current LLA: " + gps_output.gnss.geo_point.to_string(), common_utils::Utils::kLogLevelInfo);
+
+			packet.speedN = gps_output.gnss.velocity[0];
+			packet.speedE = gps_output.gnss.velocity[1];
+			packet.speedD = gps_output.gnss.velocity[2];
+
+			packet.xAccel = imu_output.linear_acceleration[0];
+			packet.yAccel = imu_output.linear_acceleration[1];
+			packet.zAccel = imu_output.linear_acceleration[2];
+
+			float yaw;
+			float pitch;
+			float roll;
+			VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw);
+			packet.yawDeg = yaw * 180.0 / M_PI;
+			packet.pitchDeg = pitch * 180.0 / M_PI;
+			packet.rollDeg = roll * 180.0 / M_PI;
+
+			Vector3r bodyRPY(roll, pitch, yaw);
+
+			// In the Unreal world, yaw is rotation around Z, so this seems to be RPY, like PySim
+			Vector3r bodyVelocityRPY(imu_output.angular_velocity[0], imu_output.angular_velocity[1], imu_output.angular_velocity[2]);
+			Vector3r earthRPY = bodyAnglesToEarthAngles(bodyRPY, bodyVelocityRPY);
+
+			packet.rollRate = earthRPY[0] * 180.0 / M_PI;
+			packet.pitchRate = earthRPY[1] * 180.0 / M_PI;
+			packet.yawRate = earthRPY[2] * 180.0 / M_PI;
+
+			// Heading appears to be unused by AruPilot.  But use yaw for now
+			packet.heading = yaw;
+
+			packet.airspeed = std::sqrt(
+				packet.speedN * packet.speedN
+				+ packet.speedE * packet.speedE
+				+ packet.speedD * packet.speedD);
+
+			packet.magic = 0x4c56414f;
+
+			if (udpSocket_ != nullptr)
+			{
+				std::vector<uint8_t> msg(sizeof(packet));
+				memcpy(msg.data(), &packet, sizeof(packet));
+				udpSocket_->sendMessage(msg);
+			}
+		}
+	}
+
+	virtual void close()
+	{
+		MavLinkMultirotorApi::close();
+
+		if (udpSocket_ != nullptr) {
+			udpSocket_->close();
+			udpSocket_->unsubscribe(rotorSubscriptionId_);
+			udpSocket_ = nullptr;
+		}
+	}
+
+protected:
+	virtual void connect()
+	{
+		if (!is_simulation_mode_) {
+
+			MavLinkMultirotorApi::connect();
+		}
+		else {
+			const std::string& ip = connection_info_.ip_address;
+			int port = connection_info_.ip_port;
+
+			close();
+
+			if (ip == "") {
+				throw std::invalid_argument("UdpIp setting is invalid.");
+			}
+
+			if (port == 0) {
+				throw std::invalid_argument("UdpPort setting has an invalid value.");
+			}
+
+			Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
+			Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
+
+			udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.sitl_ip_port);
+			mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg) {
+				this->rotorPowerMessageHandler(connection, msg);
+			};
+
+			rotorSubscriptionId_ = udpSocket_->subscribe(handler);
+		}
+}
+
+private:
+#ifdef __linux__
+	struct __attribute__((__packed__)) SensorMessage {
+#else
+#pragma pack(push,1)
+	struct SensorMessage {
+#endif
+		// this is the packet sent by the simulator
+		// to the APM executable to update the simulator state
+		// All values are little-endian
+		uint64_t timestamp;
+		double latitude, longitude; // degrees
+		double altitude;  // MSL
+		double heading;   // degrees
+		double speedN, speedE, speedD; // m/s
+		double xAccel, yAccel, zAccel;       // m/s/s in body frame
+		double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame
+		double rollDeg, pitchDeg, yawDeg;    // euler angles, degrees
+		double airspeed; // m/s
+		uint32_t magic; // 0x4c56414f
+	};
+#ifndef __linux__
+#pragma pack(pop)
+#endif
+
+	static const int kArduCopterRotorControlCount = 11;
+
+	struct RotorControlMessage {
+		// ArduPilot Solo rotor control datagram format
+		uint16_t pwm[kArduCopterRotorControlCount];
+		uint16_t speed, direction, turbulance;
+	};
+
+	std::shared_ptr<mavlinkcom::AdHocConnection> udpSocket_;
+	int rotorSubscriptionId_;
+
+	virtual void normalizeRotorControls()
+	{
+		// change 1000-2000 to 0-1.
+		for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+			rotor_controls_[i] = (rotor_controls_[i] - 1000.0f) / 1000.0f;
+		}
+	}
+
+	void rotorPowerMessageHandler(std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg)
+	{
+		if (msg.size() != sizeof(RotorControlMessage))
+		{
+			Utils::log("Got rotor control message of size " + std::to_string(msg.size()) + " when we were expecting size " + std::to_string(sizeof(RotorControlMessage)), Utils::kLogLevelError);
+			return;
+		}
+
+		RotorControlMessage rotorControlMessage;
+		memcpy(&rotorControlMessage, msg.data(), sizeof(RotorControlMessage));
+
+		std::lock_guard<std::mutex> guard_actuator(hil_controls_mutex_);    //use same mutex as HIL_CONTROl
+
+		for (auto i = 0; i < RotorControlsCount && i < kArduCopterRotorControlCount; ++i) {
+			rotor_controls_[i] = rotorControlMessage.pwm[i];
+		}
+
+		normalizeRotorControls();
+	}
+
+	Vector3r bodyAnglesToEarthAngles(Vector3r bodyRPY, Vector3r bodyVelocityRPY)
+	{
+		float p = bodyVelocityRPY[0];
+		float q = bodyVelocityRPY[1];
+		float r = bodyVelocityRPY[2];
+
+		// Roll, pitch, yaw
+		float phi = bodyRPY[0];
+		float theta = bodyRPY[1];
+
+		float phiDot = p + tan(theta)*(q*sin(phi) + r * cos(phi));
+
+		float thetaDot = q * cos(phi) - r * sin(phi);
+		if (fabs(cos(theta)) < 1.0e-20)
+		{
+			theta += 1.0e-10f;
+		}
+
+		float psiDot = (q*sin(phi) + r * cos(phi)) / cos(theta);
+
+		return Vector3r(phiDot, thetaDot, psiDot);
+	}
+
+};
+
+}
+} //namespace
+
+#endif