Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update docs #92

Merged
merged 2 commits into from
Dec 10, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 54 additions & 31 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ The key feature is to run the simulation on a real hardware and use the actual C
**Purpose**

- Testing autopilot peripheral drivers (DroneCAN or Cyphal)
- Testing real hardware components (actuators, payload, power and electrical systems) in pair with control system
- Testing hardware components (actuators, payload, power and electrical systems) in pair with autopilot
- Communication Bus and Wiring Testing
- Fault Injection and Safety Measures
- Environmental and Durability Testing: Vibration Emulation, Long-Duration Endurance Runs
Expand All @@ -20,18 +20,19 @@ The key feature is to run the simulation on a real hardware and use the actual C
**Recommended requirements for HITL:**

- Operating System: Pure Ubuntu 20.04, 22.04 or 24.04 installation is highly recommended; WSL and VM are not recommended
Operating System: A pure installation of Ubuntu 20.04, 22.04, or 24.04 is recommended; avoid WSL and VMs
- Docker: use regular Docker Engine, not Docker Desktop (because Docker Desktop relies on VM)
- Performance: for HITL part even Raspberry PI 4 is enough

**Recommended requirements for 3D simulator:**

- Operating System: We've tailored the simulator for modern versions of Windows, Linux, and Mac. Choose the build that matches your OS.
- CPU: Aim for an Intel i7 from the 11th or 12th generation. For those using AMD, any equivalent processor will suffice.
- Operating System: Modern versions of Windows, Linux, and Mac.
- CPU: Aim for an Intel i7 (from 11th/12th gen). For those using AMD, any equivalent processor will suffice.
- RAM: 16GB is a recommended minimum, but more is always better for performance.

**Required hardware:**

- Flight controller: fmu-v5, fmu-v6c or fmu-v6x
- Flight controller: fmu-v5, fmu-v6c or fmu-v6x, cuav_x7pro, cuav_nora and more by request
- CAN-sniffer: Zubax Babel is recommended, but other CAN-adapters can be suitable as well

## 1. USE CASES
Expand Down Expand Up @@ -146,18 +147,32 @@ roslaunch innopolis_vtol_dynamics 3d_sim.launch

### 1.3. Testing with Real UAV Components

Connect the HITL simulator to a particular component of a vehicle or almost to the whole system.
- Full-System Simulation: Use actual UAV hardware to simulate real-world operations without physical flight.
- Actuator and Payload Testing: Validate that motors, control surfaces, and payloads respond correctly to simulated inputs.
Connect the HITL simulator to a particular component of a vehicle or almost to the whole system. Use actual UAV hardware to simulate real-world operations without physical flight.

An example of connection to the whole system is show below.

<img src="https://github.com/ZilantRobotics/innopolis_vtol_dynamics/wiki/assets/welcome/use_case_3.png" alt="drawing" width="800"/>

### 1.4. SITL if you don't have the hardware
Example of testing scenarios:
1. Power and Electrical System Verification:
- Load Testing: Simulate various flight profiles that cause different current draws on ESCs and motors.
- Battery and Power Module Evaluation: Emulate different battery states-of-charge, voltage sags, or current spikes
2. Thermal Management Assessment:
- Thermal Stress Scenarios: By simulating prolonged hover or aggressive maneuvers, the electronics may heat up.
3. Communication Bus and Wiring Testing:
- Bus Utilization and Noise: By simulating high CAN communication traffic engineers can evaluate if cables, connectors, and bus termination are correct.
4. Fault Injection and Safety Measures:
- Fail-Safe Triggers: Simulate sensor failures, GPS dropouts, or communications errors and observe the hardware’s ability to detect and respond via built-in fail-safes, including checking the reaction of power relays, backup power lines, or redundant components.
5. Environmental and Durability Testing:
- Vibration Emulation: Although you aren’t using real propellers, you can introduce simulated vibration profiles through test stands or external actuators. This allows checking if connectors remain tight under conditions similar to real flight.
- Long-Duration Endurance Runs: Run the drone hardware in a simulated “mission” environment for hours, verifying long-term reliability, potential component drift, or intermittent connection issues that only appear after extended usage.

### 1.4. SITL (If you don't have the hardware)

SITL mode is out of the scope of the interests of this simulator. But anyway, it happens that you need to test something and you don't have the required hardware in your hand. So, you can run the flight stack and the dynamics on your PC:

[![](https://img.youtube.com/vi/bk03fAoYGfk/0.jpg)](https://youtu.be/bk03fAoYGfk)

Run the HITL dynamics as usual, but choose MAVLink mode, for example:

```bash
Expand Down Expand Up @@ -198,8 +213,6 @@ To build docker image, type:
./scripts/sim.py b # b stands for build
```

> An image on dockerhub usually is not up to date, so it's better to build manually

**Step 3. Connect everything together for HITL**

> You should skip this step if you want to run PX4 MAVLink SITL mode. Please follow [docs/px4/mavlink](docs/px4/mavlink.md) for details.
Expand Down Expand Up @@ -250,29 +263,29 @@ Here 2 options are suggested.

**Step 6. (optional) 3D Simulator**

> A new 3D simulator will appear here soon.
```bash
# 1. Run the Simulator3d itself
./ZilantSimulator.x86_64

# 2. Run rosbridge and sim interface nodes
./scripts/docker.sh i
roslaunch innopolis_vtol_dynamics 3d_sim.launch
```

## 3. SUPPORTED MODES

You can obrain the actual list of the suported modes by typing `./scripts/sim.py --help`.

Well, here is the output of the command:
Well, here is our main targets:

```bash
Primary supported modes (with aliases):
px4_v1_15_0_cyphal_quadcopter,cq | Cyphal PX4 v1.15-beta Quadrotor x (4001)
px4_v1_15_0_cyphal_quadplane_vtol,csv | Cyphal PX4 v1.15-beta Standard VTOL (13000)
px4_v1_15_0_dronecan_quadrotor,dq | DroneCAN PX4 v1.15-beta Quadrotor (4001)
px4_v1_15_0_dronecan_quadplane_vtol,dv | DroneCAN PX4 v1.15-beta Standard VTOL (13000)
px4_v1_13_0_dronecan_vtol,dv1130 | DroneCAN PX4 v1.13.0 vtol 13070

Other modes:
px4_v1_12_0_mavlink_quadplane_vtol | MAVLink PX4 v1.12 vtol 13070
px4_v1_12_0_mavlink_quadcopter | MAVLink PX4 v1.12 Quadrotor (4001)
cyphal_and_dronecan | 2 CAN AP v4.4.0 Copter
px4_v1_15_0_cyphal_octorotor,co | Cyphal PX4 v1.15-beta Octorotor Coaxial (12001)

```
| **Target** | **DroneCAN** | **Cyphal** | **MAVLink SITL** |
|----------------------|--------------|------------|------------------|
| PX4 v1.15 Quadcopter | ✅ Supported | ✅ Supported | ✅ Supported |
| PX4 v1.15 Quadplane VTOL | ✅ Supported | ✅ Supported | ✅ Supported |
| PX4 v1.15 Plane | ❌ Not Supported | ❌ Not Supported | ❌ Not Supported |
| ArduPilot v4.5.7 Copter | ✅ Supported | ❌ Not Supported | ❌ Not Supported |
| ArduPilot v4.5.7 Plane | ❌ Not Supported | ❌ Not Supported | ❌ Not Supported |
| ArduPilot v4.5.7 QuadPlane | ❌ Not Supported | ❌ Not Supported | ❌ Not Supported |

New modes will be extended step by step.

Expand All @@ -288,13 +301,23 @@ The design of the simulator is shown below.

<img src="https://github.com/ZilantRobotics/innopolis_vtol_dynamics/wiki/assets/welcome/scheme.png" alt="drawing"/>

## 5. EXAMPLE
## 5. ADDING A CUSTOM DYNAMICS

1. Create a new command in [configs/vehicles/](configs/vehicles/) folder.
- Once you create a new yaml file, the command should appear in `./scripts/sim.py --help`
2. Create a dynamics properties in [configs/dynamics/](configs/dynamics/) directory
- Use either existed multirotor or VTOL dynamics as template
3. Keep your autopilot configuration parameters in [configs/](configs/) directory
- It can be used either as a hint for manual further configuration or in an auto configuration script
4. [will be automoted soon] Add a command in [scripts/run_sim.sh](scripts/run_sim.sh).

## 6. EXAMPLE

Check the video below.

[![Cyphal/DroneCAN HITL VTOL dynamics simulator](https://img.youtube.com/vi/e9MREW6tCmE/0.jpg)](https://youtu.be/e9MREW6tCmE)

## 6. REFERENCE
## 7. REFERENCE

Docs:

Expand All @@ -307,11 +330,11 @@ Outdated manual instructions:
- [PX4 DroneCAN manual configuration instructions](docs/px4/dronecan.md)
- [ArduPilot manual configuration instructions](docs/ardupilot/README.md)

## 7. CHANGELOG NOTES
## 8. CHANGELOG NOTES

| Version | ReleaseDate | Major changes |
| ------- | ----------- | ------------- |
| v0.10.0 | In progress... | Add ArduPilot support |
| v0.10.0 | In progress... | Add ArduPilot: Quadcopter and Plane support |
| v0.9.0 | Dec 08, 2024 | Add px4_fmu-v6c, px4_fmu-v6x, cuav_x7pro, cuav_nora support beside fmu-v5 |
| v0.8.0 | Jun 10, 2024 | Update PX4 from v1.14 to v1.15 |
| v0.7.0 | Oct 31, 2023 | Update PX4 from v1.13 to v1.14 |
Expand Down
45 changes: 39 additions & 6 deletions configs/dynamics/quadcopter_ardupilot/params.yaml
Original file line number Diff line number Diff line change
@@ -1,40 +1,73 @@
# QuadCopter Dynamics Configuration for ArduCopter
# This configuration file defines the dynamics properties for a general quadcopter model
# with a total mass of 2 kg. It is designed specifically for HITL (Hardware-in-the-Loop)
# simulation in an ArduPilot environment, aiming to replicate the behavior of a real-world drone.
#
# Key Assumptions:
# - The quadcopter follows a symmetric X-frame design.
# - The drone uses four identical motors and propellers, with a thrust-to-weight ratio
# sufficient for stable flight.
# - Dynamics parameters align with the default PID tuning of ArduPilot to minimize
# the need for manual adjustments.

# The total mass of the vehicle, including the frame, motors, ESCs, and payload.
vehicle_mass : 2.0 # kg

# This represents the response time of the motor to changes in input for brushless motors with ESCs
# Higher-quality ESCs and motors may have a faster response (closer to 0.02 sec).
# Slower or less efficient systems may be closer to 0.05 sec.
motor_time_constant: 0.02 # sec

# For a standard 10–12 inch propeller
# This is the rotational inertia of the motor and propeller system.
# It depends on the size and weight of the propeller and motor rotor:
# Typical value: 0.00002–0.00006 kg·m²
# The rotational inertia of the motor and propeller system.
# Depends on the size and weight of the propeller and motor rotor.
# Typical value: 0.00002–0.00006 kg·m² for standard 10–12 inch propellers.
# Smaller drones with lighter props will have lower values.
motor_rotational_inertia: 6.62e-6 # kg m^2
motor_rotational_inertia: 6.62e-6 # kg·m²

# This coefficient relates motor rotational speed to generated thrust.
# Higher values indicate more thrust is produced for a given RPM.
thrust_coefficient: 1.91e-6 # N/(rad/s)^2"

# This coefficient relates motor rotational speed to generated torque.
# Torque impacts the drone's rotational dynamics (yaw).
torque_coefficient: 2.6e-7 # Nm/(rad/s)^2

# Represents the aerodynamic drag acting on the drone.
# Higher values indicate more resistance to motion through the air.
drag_coefficient: 0.1 # N/(m/s)

# Coefficient representing aerodynamic moments acting about the axis.
# This can arise from asymmetries in the propeller wash or frame.
aeromoment_coefficient_xx: 0.003 # Nm/(rad/s)^2
aeromoment_coefficient_yy: 0.003 # Nm/(rad/s)^2
aeromoment_coefficient_zz: 0.003 # Nm/(rad/s)^2

# The moment of inertia about the X or Y-axis, affecting roll or pitch dynamics.
# Larger values indicate more resistance to angular acceleration in roll or pitch.
vehicle_inertia_xx: 0.045 # kg * m^2
vehicle_inertia_yy: 0.045 # kg * m^2

# Low value -> the yaw oscillations.
# High value -> slow response on yaw inputs and it may feel "lazy" in maintaining the commanded yaw angle.
vehicle_inertia_zz: 0.045 # kg * m^2

# The maximum rotational speed of the propeller.
# Determines the upper limit of thrust and torque generation.
max_prop_speed: 2200 # rad/s

# The process noise for the moment and force estimation in the filter.
# Lower values indicate more confidence in model accuracy.
moment_process_noise: 1.25e-7 # (Nm)^2 s
force_process_noise: 0.0005 # N^2 s"

# The perpendicular distance between a force's line of action (such as thrust
# or lift) and the center of mass (or the axis of rotation) of the UAV
moment_arm: 0.35 # m

# The process noise for bias estimation, the initial variance in the filter and measurement noice.
accelerometer_biasprocess: 0.0 # m^2/s^5, 1.0e-7
gyroscope_biasprocess: 0.0 # rad^2/s^3, 1.0e-7
accelerometer_biasinitvar: 0.00001 # (m/s^2)^2, 0.005
gyroscope_biasinitvar: 0.00001 # (rad/s)^2, 0.003
accelerometer_variance: 0.0001 # m^2/s^4, 0.001
gyroscope_variance: 0.00001 # rad^2/s^2, 0.001
gyroscope_variance: 0.00001 # rad^2/s^2, 0.001
Loading