Added betaflight API (#31)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>

.github/workflows/ci.yaml

@@ -35,6 +35,7 @@ jobs:
             betaflight_gazebo
             gz_aerial_plugins
             vehicle_gateway
+            vehicle_gateway_betaflight
             vehicle_gateway_px4
             vehicle_gateway_python
             vehicle_gateway_worlds

README.md

@@ -32,7 +32,7 @@ sudo apt remove ros-humble-ros-gz-bridge
 ```
 
 ### Build the Vehicle Gateway
-We can now build the Vehicle Gateway itself. To keep paths short, we will make a colcon workspace named `vg` for "Vehicle Gateway", in your home directory. The Vehicle Gateway build will also download and build the PX4 firmware, to allow software-in-the-loop (SITL) simulation.
+We can now build the Vehicle Gateway itself. To keep paths short, we will make a colcon workspace named `vg` for "Vehicle Gateway", in your home directory. The Vehicle Gateway build will also download and build the PX4 firmware and Betaflight firmware, to allow software-in-the-loop (SITL) simulation of multiple autopilot software stacks.
 
 At time of writing, the `rosdep` command has to include a lot of `--skip-key` because currently Gazebo Garden is not yet in `rosdep`.
 

betaflight_controller/CMakeLists.txt

@@ -19,17 +19,17 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
-add_executable(main
+add_executable(betaflight_joy_controller
   src/main.cpp
   src/SocketBetaflight.cpp
 )
-ament_target_dependencies(main
+ament_target_dependencies(betaflight_joy_controller
   rclcpp
   sensor_msgs
 )
 
 install(TARGETS
-  main
+  betaflight_joy_controller
   DESTINATION lib/${PROJECT_NAME}
 )
 

betaflight_controller/src/main.cpp

@@ -57,10 +57,10 @@ class MinimalSubscriber : public rclcpp::Node
     pkt.channels[1] = ((msg->axes[3] + 1) / 2 * 1000) + 1000;   // pitch
     pkt.channels[2] = ((msg->axes[1] + 1) / 2 * 1000) + 1000;   // yaw
     pkt.channels[3] = ((msg->axes[0] + 1) / 2 * 1000) + 1000;   // throtlle
-    pkt.channels[4] = (msg->buttons[0] * 1000) + 1000;   // throtlle;
-    pkt.channels[5] = 1000;
-    pkt.channels[6] = 1000;
-    pkt.channels[7] = 1000;
+    pkt.channels[4] = (msg->buttons[0] * 1000) + 1000;   // aux1;
+    pkt.channels[5] = (msg->buttons[1] * 1000) + 1000;   // aux2;
+    pkt.channels[6] = (msg->buttons[2] * 1000) + 1000;   // aux3;
+    pkt.channels[7] = (msg->buttons[3] * 1000) + 1000;   // aux4;
     pkt.channels[8] = 1000;
     pkt.channels[9] = 1000;
     pkt.channels[10] = 1000;

betaflight_gazebo/src/BetaflightPlugin.cpp

@@ -63,11 +63,11 @@ GZ_ADD_PLUGIN_ALIAS(betaflight_gazebo::BetaFlightPlugin, "BetaFlightPlugin")
 struct ServoPacket
 {
   /// \brief Motor speed data.
-  /// should rename to servo_command here and in ArduPilot SIM_Gazebo.cpp
+  /// should rename to servo_command here and in Betaflight SIM_Gazebo.cpp
   float motorSpeed[MAX_MOTORS] = {0.0f};
 };
 
-/// \brief Flight Dynamics Model packet that is sent back to the ArduPilot
+/// \brief Flight Dynamics Model packet that is sent back to the Betaflight
 struct fdmPacket
 {
   /// \brief packet timestamp
@@ -87,6 +87,9 @@ struct fdmPacket
 
   /// \brief Model position in NEED frame
   double positionXYZ[3];
+
+  /// \brief Pressure value
+  double pressure;
 };
 
 /// \brief Control class
@@ -290,12 +293,12 @@ class betaflight_gazebo::BetaFlightPluginPrivate
 public:
   std::mutex mutex;
   //
-  /// \brief Ardupilot Socket for receive motor command on gazebo
+  /// \brief Betaflight Socket for receive motor command on gazebo
 
 public:
   BetaflightSocket socket_in;
 
-  /// \brief Ardupilot Socket to send state to Ardupilot
+  /// \brief Betaflight Socket to send state to Betaflight
 
 public:
   BetaflightSocket socket_out;
@@ -331,6 +334,15 @@ class betaflight_gazebo::BetaFlightPluginPrivate
     imuMsgValid = true;
   }
 
+  float pressure = 101325;    // pressure in Pa (0m MSL);
+  std::mutex airPressureMsgMutex;
+
+  void onAirPressureMessageReceived(const gz::msgs::FluidPressure & _msg)
+  {
+    std::lock_guard<std::mutex> lock(this->airPressureMsgMutex);
+    this->pressure = _msg.pressure();
+  }
+
   /// \brief keep track of controller update sim-time.
 
 public:
@@ -346,7 +358,7 @@ class betaflight_gazebo::BetaFlightPluginPrivate
 public:
   bool isLockStep{false};
 
-  /// \brief false before ardupilot controller is online
+  /// \brief false before Betaflight controller is online
   /// to allow gazebo to continue without waiting
 
 public:
@@ -468,6 +480,12 @@ void betaflight_gazebo::BetaFlightPlugin::Configure(
       sdfClone->Get<gz::math::Pose3d>("gazeboXYZToNED");
   }
 
+  // TODO(ahcorde): fix this topic name
+  this->dataPtr->node.Subscribe(
+    "/world/empty_betaflight_world/model/iris_with_Betaflight/model/iris_with_standoffs/"
+    "link/imu_link/sensor/air_pressure_sensor/air_pressure",
+    &BetaFlightPluginPrivate::onAirPressureMessageReceived, this->dataPtr.get());
+
   // Load control channel params
   this->LoadControlChannels(sdfClone, _ecm);
 
@@ -1079,7 +1097,7 @@ bool betaflight_gazebo::BetaFlightPlugin::ReceiveServoPacket(
     // gazebo::common::Time::NSleep(100);
     if (this->dataPtr->betaflightOnline) {
       // gzwarn << "[" << this->dataPtr->modelName << "] "
-      //        << "Broken ArduPilot connection, count ["
+      //        << "Broken Betaflight connection, count ["
       //        << this->dataPtr->connectionTimeoutCount
       //        << "/" << this->dataPtr->connectionTimeoutMaxCount
       //        << "]\n";
@@ -1093,7 +1111,7 @@ bool betaflight_gazebo::BetaFlightPlugin::ReceiveServoPacket(
         } else {
           this->dataPtr->betaflightOnline = false;
           gzwarn << "[" << this->dataPtr->modelName << "] "
-                 << "Broken ArduPilot connection, resetting motor control.\n";
+                 << "Broken Betaflight connection, resetting motor control.\n";
           this->ResetPIDs();
         }
       }
@@ -1110,7 +1128,7 @@ bool betaflight_gazebo::BetaFlightPlugin::ReceiveServoPacket(
 
     if (!this->dataPtr->betaflightOnline) {
       gzdbg << "[" << this->dataPtr->modelName << "] "
-            << "ArduPilot controller online detected.\n";
+            << "Betaflight controller online detected.\n";
       // made connection, set some flags
       this->dataPtr->connectionTimeoutCount = 0;
       this->dataPtr->betaflightOnline = true;
@@ -1175,6 +1193,11 @@ void betaflight_gazebo::BetaFlightPlugin::SendState(
     imuMsg = this->dataPtr->imuMsg;
   }
 
+  {
+    std::lock_guard<std::mutex> lock(this->dataPtr->airPressureMsgMutex);
+    pkt.pressure = this->dataPtr->pressure;
+  }
+
   // asssumed that the imu orientation is:
   //   x forward
   //   y right
@@ -1214,12 +1237,11 @@ void betaflight_gazebo::BetaFlightPlugin::SendState(
     _ecm.Component<gz::sim::components::WorldLinearVelocity>(
     this->dataPtr->imuLink);
 
-
   // get inertial pose and velocity
   // position of the uav in world frame
   // this position is used to calculate bearing and distance
   // from starting location, then use that to update gps position.
-  // The algorithm looks something like below (from ardupilot helper
+  // The algorithm looks something like below (from Betaflight helper
   // libraries):
   //   bearing = to_degrees(atan2(position.y, position.x));
   //   distance = math.sqrt(self.position.x**2 + self.position.y**2)

betaflight_sim/CMakeLists.txt

@@ -22,10 +22,6 @@ externalproject_add(betaflight-firmware
   BUILD_COMMAND pwd && ${MAKE_EXECUTABLE} TARGET=SITL -j
   BUILD_IN_SOURCE ON
   CONFIGURE_COMMAND ""
-  PATCH_COMMAND git checkout . &&
-  patch -p1 < ${CMAKE_CURRENT_SOURCE_DIR}/patches/target.hh.patch &&
-  patch -p1 < ${CMAKE_CURRENT_SOURCE_DIR}/patches/udp.cc.patch &&
-    patch -p1 < ${CMAKE_CURRENT_SOURCE_DIR}/patches/tools.mk.patch
 )
 
 ament_environment_hooks(cmake/99_betaflight_setup.sh.in)

betaflight_sim/launch/quadcopter.launch.py

@@ -16,6 +16,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -28,30 +29,11 @@ def get_betaflight_dir():
     return get_package_share_directory('betaflight_sim')
 
 
-run_betaflight_controller = Node(
-    package='betaflight_controller',
-    executable='main',
-    output='screen')
+run_virtual_tty = ExecuteProcess(cmd=["socat", "-dd", "pty,link=/tmp/ttyS0,raw,echo=0",
+                                      "tcp:127.0.0.1:5761"])
 
-run_betaflight_sitl = ExecuteProcess(
-    cmd=['betaflight_SITL.elf', "127.0.0.1"],
-    cwd=os.path.join(get_betaflight_dir(), "config"),
-    output='screen')
 
-
-def _run_betaflight_sitl_check(event):
-    """
-    Consider betaflight_controller ready when 'Opened joystick...' string is printed.
-
-    Launches betaflight_controller node if ready.
-    """
-    target_str = 'Opened joystick'
-    if target_str in event.text.decode():
-        time.sleep(5)
-        return run_betaflight_sitl
-
-
-def _run_betaflight_controller_check(event):
+def _run_virtual_tty_check(event):
     """
     Consider betaflight_controller ready when 'bind port 5761 for UART1...' string is printed.
 
@@ -60,7 +42,7 @@ def _run_betaflight_controller_check(event):
     target_str = 'bind port 5761 for UART1'
     if target_str in event.text.decode():
         time.sleep(2)
-        return run_betaflight_controller
+        return run_virtual_tty
 
 
 def generate_launch_description():
@@ -73,14 +55,17 @@ def generate_launch_description():
         description='If true, use simulated clock')
 
     os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.join(get_betaflight_dir(), "models")
+    os.environ["GZ_SIM_RESOURCE_PATH"] += ":" + os.path.join(get_betaflight_dir(), "worlds")
 
     world_sdf = os.path.join(get_betaflight_dir(), "worlds", "empty_betaflight_world.sdf")
 
-    joy_node = Node(
-        package='joy',
-        executable='joy_node',
-        parameters=[{'use_sim_time': use_sim_time}],
-        output='screen')
+    use_groundcontrol = DeclareLaunchArgument('groundcontrol', default_value='false',
+                                              choices=['true', 'false'],
+                                              description='Start ground control station.')
+
+    run_betaflight_sitl = ExecuteProcess(cmd=['betaflight_SITL.elf', "127.0.0.1"],
+                                         cwd=os.path.join(get_betaflight_dir(), "config"),
+                                         output='screen')
 
     # Bridge
     bridge = Node(
@@ -97,22 +82,17 @@ def generate_launch_description():
                 [os.path.join(get_package_share_directory('ros_gz_sim'),
                               'launch', 'gz_sim.launch.py')]),
             launch_arguments=[('gz_args', [' -r -v 4 ' + world_sdf])]),
-        joy_node,
         use_sim_time_arg,
+        run_betaflight_sitl,
+        use_groundcontrol,
+        ExecuteProcess(cmd=['betaflight-configurator'],
+                       condition=IfCondition(LaunchConfiguration('groundcontrol'))),
         bridge,
-        RegisterEventHandler(
-            OnProcessIO(
-                target_action=joy_node,
-                on_stdout=_run_betaflight_sitl_check,
-                on_stderr=_run_betaflight_sitl_check
-            )
-        ),
         RegisterEventHandler(
             OnProcessIO(
                 target_action=run_betaflight_sitl,
-                on_stdout=_run_betaflight_controller_check,
-                on_stderr=_run_betaflight_controller_check
+                on_stdout=_run_virtual_tty_check,
+                on_stderr=_run_virtual_tty_check
             )
         ),
-        ExecuteProcess(cmd=['betaflight-configurator']),
     ])