getRCData, setRCData APIs

AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp

@@ -45,11 +45,13 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     Vector3r getPosition();
     Vector3r getVelocity();
     Quaternionr getOrientation();
-    RCData getRCData();
     GeoPoint getGpsLocation();
     bool isSimulationMode();
     std::string getDebugInfo();
 
+    RCData getRCData();
+    void setRCData(const RCData& rc_data);
+
     DroneControllerBase::LandedState getLandedState();
 
     bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,

AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp

@@ -202,6 +202,8 @@ class DroneControllerBase : public VehicleControllerBase {
     /// Get the current RC inputs when RC transmitter is talking to to flight controller
     virtual RCData getRCData() = 0;
 
+    virtual RCData estimateRCTrims(CancelableBase& cancelable_action, float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100);
+
     /// Set the RC data that should be used by flight controller
     virtual void setRCData(const RCData& rcData) = 0;
 
@@ -326,6 +328,8 @@ class DroneControllerBase : public VehicleControllerBase {
         }
     };
 
+    RCData rc_data_trims_;
+
 private: //methods
     float setNextPathPosition(const vector<Vector3r>& path, const vector<PathSegment>& path_segs,
         const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc);

AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -154,11 +154,12 @@ class SimpleFlightDroneController : public DroneControllerBase {
 
     virtual RCData getRCData() override
     {
-        return RCData();
+        return last_rcData_;
     }
 
     virtual void setRCData(const RCData& rcData) override
     {
+        last_rcData_ = rcData;
         if (rcData.is_valid) {
             board_->setIsRcConnected(true);
             board_->setInputChannel(0, rcData.roll); //X
@@ -335,6 +336,8 @@ class SimpleFlightDroneController : public DroneControllerBase {
     unique_ptr<simple_flight::IFirmware> firmware_;
 
     VehicleParams safety_params_;
+
+    RCData last_rcData_;
 };
 
 }} //namespace

AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp

@@ -150,6 +150,10 @@ RCData MultirotorRpcLibClient::getRCData()
 {
     return static_cast<rpc::client*>(getClient())->call("getRCData").as<MultirotorRpcLibAdapators::RCData>().to();
 }
+void MultirotorRpcLibClient::setRCData(const RCData& rc_data)
+{
+    static_cast<rpc::client*>(getClient())->call("setRCData", MultirotorRpcLibAdapators::RCData(rc_data));
+}
 
 GeoPoint MultirotorRpcLibClient::getGpsLocation()
 {

AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp

@@ -397,47 +397,62 @@ bool DroneControllerBase::setSafety(SafetyEval::SafetyViolationType enable_reaso
     return true;
 }
 
-bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action)
-{
-    const float kMaxMessageAge = 10000000.0f /* 0.1 sec */, kTrimduration = 1, kMinCountForTrim = 10, kMaxTrim = 100, kMaxRCValue = 10000;
-
-    if (duration <= 0)
-        return true;
 
-    //freeze the quaternion
-    Quaternionr starting_quaternion = getOrientation();
+RCData DroneControllerBase::estimateRCTrims(CancelableBase& cancelable_action, float trimduration, float minCountForTrim, float maxTrim)
+{
+    rc_data_trims_ = RCData();
 
     //get trims
-    Waiter waiter_trim(getCommandPeriod(), kTrimduration);
-    RCData rc_data_trims;
+    Waiter waiter_trim(getCommandPeriod(), trimduration);
     uint count = 0;
     do {
 
         const RCData rc_data = getRCData();
-        if (rc_data.timestamp > 0) {
-            rc_data_trims.add(rc_data);
+        if (rc_data.is_valid) {
+            rc_data_trims_.add(rc_data);
             count++;
         }
 
     } while (waiter_trim.sleep(cancelable_action) && !waiter_trim.is_timeout());
 
-    if (count < kMinCountForTrim)
-        throw VehicleMoveException("Cannot compute RC trim because too few readings received");
+    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<float>(count));
-    if (rc_data_trims.isAnyMoreThan(kMaxTrim))
-        throw VehicleMoveException(Utils::stringf("RC trims does not seem to be valid: %s", rc_data_trims.toString().c_str()));
+    rc_data_trims_.divideBy(static_cast<float>(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()));
+    Utils::log(Utils::stringf("RCData Trims: %s", rc_data_trims_.toString().c_str()));
+
+    return rc_data_trims_;
+}
+
+bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action)
+{
+    const float kMaxMessageAge = 0.1f /* 0.1 sec */, kMaxRCValue = 10000;
+
+    if (duration <= 0)
+        return true;
+
+    //freeze the quaternion
+    Quaternionr starting_quaternion = getOrientation();
 
     Waiter waiter(getCommandPeriod(), duration);
     do {
 
         RCData rc_data = getRCData();
         TTimeDelta age = clock()->elapsedSince(rc_data.timestamp);
-        if (age <= kMaxMessageAge) {
-            rc_data.subtract(rc_data_trims);
+        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);

PythonClient/AirSimClient.py

@@ -101,18 +101,9 @@ def __init__(self, is_rate = True, yaw_or_rate = 0.0):
 
 class RCData(MsgpackMixin):
     timestamp = 0
-    pitch = 0.0 
-    roll = 0.0 
-    throttle = 0.0 
-    yaw = 0.0 
-    switch1 = 0
-    switch2 = 0 
-    switch3 = 0 
-    switch4 = 0 
-    switch5 = 0
-    switch6 = 0 
-    switch7 = 0 
-    switch8 = 0
+    pitch, roll, throttle, yaw = (0.0,)*4 #init 4 variable to to 0.0
+    switch1, switch2, switch3, switch4 = (0,)*4
+    switch5, switch6, switch7, switch8 = (0,)*4
     is_initialized = False
     is_valid = False
     def __init__(self, timestamp = 0, pitch = 0.0, roll = 0.0, throttle = 0.0, yaw = 0.0, switch1 = 0,
@@ -507,7 +498,7 @@ def hover(self):
 
 
     # query vehicle state
-    def getMultirotorState(self):
+    def getMultirotorState(self) -> MultirotorState:
         return MultirotorState.from_msgpack(self.client.call('getMultirotorState'))
     def getPosition(self):
         return Vector3r.from_msgpack(self.client.call('getPosition'))

PythonClient/PythonClient.pyproj

@@ -5,7 +5,7 @@
     <SchemaVersion>2.0</SchemaVersion>
     <ProjectGuid>e2049e20-b6dd-474e-8bca-1c8dc54725aa</ProjectGuid>
     <ProjectHome>.</ProjectHome>
-    <StartupFile>hello_drone.py</StartupFile>
+    <StartupFile>hello_car.py</StartupFile>
     <SearchPath>
     </SearchPath>
     <WorkingDirectory>.</WorkingDirectory>
@@ -46,6 +46,9 @@
     <Compile Include="drone_stress_test.py">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="manual_mode_demo.py">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="objects.py">
       <SubType>Code</SubType>
     </Compile>

PythonClient/manual_mode_demo.py

@@ -0,0 +1,29 @@
+"""
+For connecting to the AirSim drone environment and testing API functionality
+"""
+
+import os
+import tempfile
+import pprint
+
+from AirSimClient import *
+
+
+# connect to the AirSim simulator
+client = MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+client.armDisarm(True)
+
+state = client.getMultirotorState()
+s = pprint.pformat(state)
+print("state: %s" % s)
+
+client.moveByManual(vx_max = 1E6, vy_max = 1E6, z_min = -1E6, duration = 1E10)
+AirSimClientBase.wait_key('Manual mode is setup. Press any key to send RC data to takeoff')
+
+client.setRCData(rcdata = RCData(pitch = 0.0, throttle = 1.0, is_initialized = True, is_valid = True))
+
+AirSimClientBase.wait_key('Set Yaw and pitch to 0.5')
+
+client.setRCData(rcdata = RCData(roll = 0.5, throttle = 1.0, yaw = 0.5, is_initialized = True, is_valid = True))