Betaflight+Gazebo 软件在环仿真教程

ros2 --version       # 输出应包含 "humble"
gz sim --versions    # 输出应为 "Gazebo Sim 8.9.0"
dpkg -l | grep ros-humble-ros-gzharmonic  # 应显示已安装

cd ~
git clone https://github.com/Jinyao-Chen/bf_sitl.git ~/bf_sitl

~/bf_sitl/
├── autonomy/
│   ├── msp_reader.py                    # MSP 遥测读取器
│   ├── msp_closed_loop_controller.py    # 闭环定高控制器
│   └── autonomous_controller.py         # 开环起飞控制器
├── worlds/
│   └── betaflight_world.sdf             # Gazebo 世界文件
├── config/
│   └── bf_cli_config.py                 # 首次配置脚本
└── betaflight_gazebo_patches/           # 插件补丁（供编译参考）

pip3 install --user websockify
sudo apt-get install -y socat

# Gazebo Harmonic 开发包——编译自定义 Gazebo 插件必需！
sudo apt-get install -y \
    libgz-sim8-dev \
    libgz-transport13-dev \
    libgz-msgs10-dev \
    libgz-math7-dev \
    libgz-plugin2-dev \
    libsdformat14-dev

// fdm_packet: Gazebo 插件 → SITL
typedef struct {
    double timestamp;
    double imu_angular_velocity_rpy[3];    // 角速度 (rad/s)
    double imu_linear_acceleration_xyz[3]; // 线加速度 (m/s²)
    double imu_orientation_quat[4];        // 姿态四元数 (w,x,y,z)
    double velocity_xyz[3];                // 速度 (m/s, ENU 坐标系)
    double position_xyz[3];                // 位置
    double pressure;                       // 气压 (Pa)
} fdm_packet;

// servo_packet: SITL → Gazebo 插件
typedef struct {
    float motor_speed[4];   // [0.0, 1.0] 或 3D 模式 [-1.0, 1.0]
} servo_packet;

// rc_packet: Controller → SITL
typedef struct {
    double timestamp;
    uint16_t channels[16];   // PWM [1000, 2000]
} rc_packet;

cd ~
git clone https://github.com/betaflight/betaflight.git
cd betaflight

make TARGET=SITL -j$(nproc)

Linking SITL
   text    data     bss     dec     hex  filename
 389784   21804   77376  488964   77604  ./obj/main/betaflight_SITL.elf

./obj/main/betaflight_SITL.elf --help
# 输出: Betaflight SITL Usage: ./obj/main/betaflight_SITL.elf [options]

mkdir -p ~/ros2_ws/src

cd ~/ros2_ws/src
git clone https://github.com/osrf/vehicle_gateway.git vehicle_gateway

cd ~/ros2_ws/src/vehicle_gateway
for pkg in gz_aerial_plugins vehicle_gateway vehicle_gateway_betaflight \
    vehicle_gateway_demo vehicle_gateway_integration_test vehicle_gateway_multi \
    vehicle_gateway_px4 vehicle_gateway_python vehicle_gateway_python_helpers \
    vehicle_gateway_sim_performance px4_sim betaflight_configurator \
    betaflight_demo qgroundcontrol; do
    touch "$pkg/COLCON_IGNORE"
done

#include <functional>
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/fluid_pressure.pb.h>
#include <gz/msgs/double.pb.h>

std::function<void(const gz::msgs::IMU&)> imuCb =
    std::bind(&BetaFlightPluginPrivate::ImuCb,
              this->dataPtr.get(), std::placeholders::_1);
this->dataPtr->node.Subscribe(imuTopicName, imuCb);

std::function<void(const gz::msgs::FluidPressure&)> airPressureCb =
    std::bind(&BetaFlightPluginPrivate::onAirPressureMessageReceived,
              this->dataPtr.get(), std::placeholders::_1);
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",
    airPressureCb);

if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime)
{
    double t = ...;
    this->SendState(t, _ecm);
    ...
}

cd ~/ros2_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select betaflight_gazebo

ln -sf ~/ros2_ws/src/vehicle_gateway/betaflight_sim/models/iris_with_standoffs \
    ~/ros2_ws/src/vehicle_gateway/betaflight_sim/models/

<plugin name="BetaFlightPlugin" filename="BetaflightPlugin">
    <fdm_addr>127.0.0.1</fdm_addr>
    <fdm_port_in>9002</fdm_port_in>
    <listen_addr>127.0.0.1</listen_addr>
    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
    <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
    <control channel="0">
        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
        <multiplier>838</multiplier>
    </control>
    <!-- channels 1-3 similarly configured, multipliers: 838, -838, -838 -->
</plugin>

cd ~/bf_sitl/config
rm -f eeprom.bin

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

python3 ~/bf_sitl/config/bf_cli_config.py

#!/usr/bin/env python3
"""Configure Betaflight SITL via CLI — set ARM on AUX1"""
import socket, time, sys

HOST, PORT = '127.0.0.1', 5761

def recv_all(sock, timeout=1.0):
    """读取所有可用数据，超时后返回"""
    sock.settimeout(timeout)
    data = b''
    try:
        while True:
            chunk = sock.recv(4096)
            if not chunk: break
            data += chunk
    except socket.timeout: pass
    except Exception: pass
    return data

def main():
    # 1. 等待 SITL 的 TCP 端口就绪（最多等 30 秒）
    print("Waiting for SITL TCP port 5761...")
    for i in range(30):
        try:
            s = socket.socket(); s.settimeout(1)
            s.connect((HOST, PORT)); s.close()
            print(f"Connected after {i+1}s"); break
        except Exception:
            time.sleep(1)
    else:
        print("ERROR: SITL not ready after 30s"); sys.exit(1)

    # 2. 建立 TCP 连接，排空初始欢迎信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.settimeout(5)
    sock.connect((HOST, PORT))
    time.sleep(1)
    recv_all(sock, 0.5)

    # 3. 发送单独一个 '#' 进入 CLI 模式
    #    注意：只发 '#'，不带 \n。Betaflight 收到 '#'（而非 MSP 帧头 '$'）时进入 CLI
    print("Entering CLI mode...")
    sock.sendall(b'#')
    time.sleep(2)
    resp = recv_all(sock, 1.0)
    if resp:
        print(resp.decode('utf-8', errors='replace')[:300])

    # 4. 配置 ARM 到 AUX1（通道值 1700-2100 时触发 ARM 模式）
    #    aux 0 0 0 1700 2100 参数：
    #      槽位0, 模式0(ARM), AUX通道0(AUX1=第5通道), 触发范围1700-2100μs
    print("\nSetting ARM on AUX1 (1700-2100)...")
    sock.sendall(b'aux 0 0 0 1700 2100\n')
    time.sleep(0.5)
    resp = recv_all(sock, 1.0)
    if resp:
        print(resp.decode('utf-8', errors='replace')[-200:])

    # 5. 保存到 eeprom.bin 并重启飞控固件
    print("Saving to EEPROM...")
    try:
        sock.sendall(b'save\n')
        time.sleep(1)
        resp = recv_all(sock, 3.0)
        if resp:
            print(resp.decode('utf-8', errors='replace')[-300:])
    except Exception as e:
        print(f"Save: {e} (reboot is normal)")

    try: sock.close()
    except: pass
    print("\nDone! ARM on AUX1 configured.")

if __name__ == '__main__':
    main()

~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

aux 1 1 1 1500 2100

cd ~/bf_sitl/config
~/betaflight/obj/main/betaflight_SITL.elf --ip 127.0.0.1

export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ros2_ws/install/betaflight_gazebo/lib
export GZ_SIM_RESOURCE_PATH=$HOME/ros2_ws/src/vehicle_gateway/betaflight_sim/models
gz sim -r -v 4 ~/bf_sitl/worlds/betaflight_world.sdf

# 闭环定高控制器（推荐，需 MSP 遥测）
python3 ~/bf_sitl/autonomy/msp_closed_loop_controller.py \
    --target-alt 2.0 --hover-base 1450 --kp 80 --ki 5 --kd 40

# 或开环起飞控制器（无需 MSP 遥测，仅 RC 控制）
python3 ~/bf_sitl/autonomy/autonomous_controller.py --hover-throttle 1450

#!/usr/bin/env python3
"""
Closed-loop altitude hold controller for Betaflight SITL.
- Reads drone state via MSP over TCP 5761 (altitude, attitude, IMU, battery)
- Sends RC via UDP 9004 (100Hz)
- PID altitude control with attitude monitoring

This is the SAME architecture used on real hardware (TCP -> UART serial).
"""
import socket, struct, time, signal, sys, argparse, threading
from msp_reader import MSPReader

BF_IP, BF_PORT = '127.0.0.1', 9004
NUM_CH = 16

class ClosedLoopController:
    def __init__(self, target_alt=2.0, hover_base=1450, kp=80, ki=5, kd=40):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.msp = MSPReader()
        self.target_alt = target_alt
        self.hover_base = hover_base
        self.kp, self.ki, self.kd = kp, ki, kd
        self.integral = 0.0
        self.last_error = 0.0
        self.lock = threading.Lock()
        self.throttle = 1000
        self.aux1 = 1000
        self.running = True
        signal.signal(signal.SIGINT, self.stop)
        signal.signal(signal.SIGTERM, self.stop)

    def stop(self, *args):
        print("\nDisarming...")
        self.running = False
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(0.3)
        self.msp.stop()
        self.sock.close()
        sys.exit(0)

    def send_rc(self, channels):
        ts = time.time()
        pkt = struct.pack('<d' + 'H' * NUM_CH, ts, *channels)
        try:
            self.sock.sendto(pkt, (BF_IP, BF_PORT))
        except:
            pass

    def rc_thread(self):
        """Send RC at 100Hz independently of state reading"""
        while self.running:
            with self.lock:
                t, a = self.throttle, self.aux1
            ch = [1500] * NUM_CH
            ch[2], ch[4] = t, a
            self.send_rc(ch)
            time.sleep(0.01)

    def run(self):
        print("=" * 60)
        print("Betaflight SITL Closed-Loop Controller (MSP + UDP)")
        print(f"Target altitude: {self.target_alt}m")
        print(f"Hover base: {self.hover_base}  PID: kp={self.kp} ki={self.ki} kd={self.kd}")
        print("=" * 60)

        if not self.msp.start():
            print("ERROR: Failed to connect MSP reader. Is SITL running?")
            sys.exit(1)

        threading.Thread(target=self.rc_thread, daemon=True).start()

        # Phase 1: Disarmed init (3s)
        print("\nPhase 1: Init (disarmed, 3s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 1000
        time.sleep(3)

        # Phase 2: Arm (2s) -- throttle MUST be 1000 for arming
        print("Phase 2: Arming (2s)...")
        with self.lock:
            self.throttle = 1000
            self.aux1 = 2000
        time.sleep(2)

        # Phase 3: Altitude hold with PID
        print(f"Phase 3: Altitude hold at {self.target_alt}m...")
        last_time = time.time()

        while self.running:
            state = self.msp.get_state()
            now = time.time()
            dt = now - last_time
            if dt <= 0 or dt > 1.0:
                dt = 0.05

            # Get altitude from MSP (cm -> m).
            # Betaflight reports NED (positive=down), negate so "up" is positive.
            alt = -state.get('alt_cm', 0) / 100.0 if state else 0.0

            # PID control
            error = self.target_alt - alt
            self.integral += error * dt
            self.integral = max(-200, min(200, self.integral))
            deriv = (error - self.last_error) / dt
            pid = self.kp * error + self.ki * self.integral + self.kd * deriv
            thr = int(self.hover_base + pid)
            thr = max(1000, min(1900, thr))

            with self.lock:
                self.throttle = thr
                self.aux1 = 2000

            # Display
            roll = state.get('roll_deg', 0)
            pitch = state.get('pitch_deg', 0)
            volt = state.get('voltage_v', 0)
            rssi = state.get('rssi', 0)
            mode = state.get('flight_mode', 0)
            armed = bool(mode & (1 << 0)) if 'flight_mode' in state else False

            print(f"\r  alt={alt:.2f}m err={error:+.2f} thr={thr} "
                  f"r={roll:.0f}° p={pitch:.0f}° "
                  f"v={volt:.1f}V RSSI={rssi} armed={armed}  ",
                  end='', flush=True)

            self.last_error = error
            last_time = now
            time.sleep(0.05)

def main():
    p = argparse.ArgumentParser()
    p.add_argument('--target-alt', type=float, default=2.0)
    p.add_argument('--hover-base', type=int, default=1450)
    p.add_argument('--kp', type=float, default=80)
    p.add_argument('--ki', type=float, default=5)
    p.add_argument('--kd', type=float, default=40)
    args = p.parse_args()
    ClosedLoopController(args.target_alt, args.hover_base,
                         args.kp, args.ki, args.kd).run()

if __name__ == '__main__':
    main()

#!/usr/bin/env python3
"""
MSP telemetry reader for Betaflight SITL.
Connects to TCP 5761 and queries multiple MSP commands to read full drone state.
"""
import socket, struct, time, threading, select

class MSPReader:
    """Connects to SITL TCP 5761 and reads full drone telemetry via MSP"""

    MSP_CMDS = {
        'altitude': (109, '<ih'),           # alt_cm (int32), vario_cm_s (int16)
        'attitude': (108, '<hhh'),          # roll_ddeg, pitch_ddeg, yaw_deg
        'raw_imu':  (102, '<hhhhhhhhh'),    # acc[3], gyro[3], mag[3] (raw int16)
        'status':   (101, '<HHHIB'),       # cycleTime, i2cErr, sensors, mode, profile
        'analog':   (110, '<BHHh'),         # voltage_dV, mAh, rssi, amperage_cA
    }

    def __init__(self, host='127.0.0.1', port=5761):
        self.state = {}
        self.state_lock = threading.Lock()
        self.running = False
        self.host, self.port = host, port
        self.sock = None

    def connect(self, timeout=10):
        deadline = time.time() + timeout
        while time.time() < deadline:
            try:
                self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.sock.settimeout(0.5)
                self.sock.connect((self.host, self.port))
                print(f"[MSP] Connected to {self.host}:{self.port}")
                return True
            except:
                self.sock.close()
                time.sleep(0.5)
        print("[MSP] Connection timeout")
        return False

    def _build_msp_request(self, cmd):
        """Build MSPv1 request frame (no payload). CRC = cmd ^ 0x00."""
        return b'$M<' + bytes([0]) + bytes([cmd]) + bytes([cmd])

    def _parse_msp_response(self, data, cmd_id, fmt):
        """Parse MSP response. Handles both v1 and v2 framing."""
        idx = data.find(b'$M>')
        if idx < 0:
            idx = data.find(b'$X>')
        if idx < 0:
            return None
        if data[idx:idx+3] == b'$M>':
            if idx + 5 > len(data): return None
            size = data[idx + 3]
            cmd = data[idx + 4]
            if cmd != cmd_id or idx + 5 + size > len(data): return None
            payload = data[idx + 5: idx + 5 + size]
            try:
                # slice to exact format size — MSP payload may be larger
                return struct.unpack(fmt, payload[:struct.calcsize(fmt)])
            except: return None
        elif data[idx:idx+3] == b'$X>':
            if idx + 8 > len(data): return None
            cmd = data[idx + 4] | (data[idx + 5] << 8)
            size = data[idx + 6] | (data[idx + 7] << 8)
            if cmd != cmd_id or idx + 8 + size > len(data): return None
            payload = data[idx + 8: idx + 8 + size]
            try:
                return struct.unpack(fmt, payload[:struct.calcsize(fmt)])
            except: return None
        return None

    def _read_all(self):
        data = b''
        try:
            while True:
                ready, _, _ = select.select([self.sock], [], [], 0.01)
                if not ready: break
                chunk = self.sock.recv(4096)
                if not chunk: break
                data += chunk
        except: pass
        return data

    def _send_and_recv(self, cmd_id, fmt):
        try:
            self.sock.sendall(self._build_msp_request(cmd_id))
            for _ in range(5):
                time.sleep(0.02)
                data = self._read_all()
                if data:
                    result = self._parse_msp_response(data, cmd_id, fmt)
                    if result is not None: return result
        except: pass
        return None

    def query_all(self):
        result = {}
        r = self._send_and_recv(109, '<ih')
        if r: result['alt_cm'] = r[0]; result['vario_cm_s'] = r[1]
        r = self._send_and_recv(108, '<hhh')
        if r: result['roll_deg'] = r[0] / 10.0; result['pitch_deg'] = r[1] / 10.0; result['yaw_deg'] = r[2]
        r = self._send_and_recv(102, '<hhhhhhhhh')
        if r: result['acc_x'] = r[0]; result['acc_y'] = r[1]; result['acc_z'] = r[2]; result['gyro_x'] = r[3]; result['gyro_y'] = r[4]; result['gyro_z'] = r[5]
        r = self._send_and_recv(101, '<HHHIB')
        if r: result['cycle_time_us'] = r[0]; result['sensors'] = r[2]; result['flight_mode'] = r[3]; result['pid_profile'] = r[4]
        r = self._send_and_recv(110, '<BHHh')
        if r: result['voltage_v'] = r[0] / 10.0; result['mah'] = r[1]; result['rssi'] = r[2]
        return result

    def reader_thread(self):
        self.running = True
        while self.running:
            result = self.query_all()
            if result:
                with self.state_lock: self.state.update(result)
            time.sleep(0.05)

    def start(self):
        if not self.connect(): return False
        t = threading.Thread(target=self.reader_thread, daemon=True); t.start()
        return True

    def stop(self):
        self.running = False
        if self.sock: self.sock.close()

    def get_state(self):
        with self.state_lock: return dict(self.state)