From c1788e2c75d15762ebf55cbb5d13f3668ba47e61 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 27 Aug 2025 03:19:26 +0300 Subject: [PATCH] Refactor arming logic Arm and disarm with gestures only: left stick right/down for arming, left/down for disarming. Remove arming switch as it complicates arming gestures logic. Remove MAV_CTRL_SCALE parameter as it complicates arming gestures logic, advise to decrease TILT_MAX when controlling with a smartphone. Put some minimal thrust to motors to indicate armed state. Rename build article to usage article, add flight instructions. --- README.md | 2 +- docs/build.md | 206 +--------------------------------- docs/firmware.md | 2 +- docs/img/arming.svg | 22 ++++ docs/img/controls.svg | 123 ++++++++++++++++++++ docs/img/disarming.svg | 22 ++++ docs/troubleshooting.md | 2 +- docs/usage.md | 243 ++++++++++++++++++++++++++++++++++++++++ flix/cli.ino | 8 ++ flix/control.ino | 22 +++- flix/failsafe.ino | 15 +-- flix/flix.ino | 2 +- flix/mavlink.ino | 10 +- flix/parameters.ino | 5 - flix/rc.ino | 7 +- gazebo/README.md | 2 +- gazebo/flix.h | 3 +- 17 files changed, 451 insertions(+), 245 deletions(-) mode change 100644 => 120000 docs/build.md create mode 100644 docs/img/arming.svg create mode 100644 docs/img/controls.svg create mode 100644 docs/img/disarming.svg create mode 100644 docs/usage.md diff --git a/README.md b/README.md index 8e378fd..15c3ecd 100644 --- a/README.md +++ b/README.md @@ -55,7 +55,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code: ## Articles * [Assembly instructions](docs/assembly.md). -* [Building and running the code](docs/build.md). +* [Usage: build, setup and flight](docs/usage.md). * [Troubleshooting](docs/troubleshooting.md). * [Firmware architecture overview](docs/firmware.md). * [Python library tutorial](tools/pyflix/README.md). diff --git a/docs/build.md b/docs/build.md deleted file mode 100644 index d1cd00c..0000000 --- a/docs/build.md +++ /dev/null @@ -1,205 +0,0 @@ -# Building and running - -To build the firmware or the simulator, you need to clone the repository using git: - -```bash -git clone https://github.com/okalachev/flix.git -cd flix -``` - -## Simulation - -### Ubuntu - -The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine. - -1. Install Arduino CLI: - - ```bash - curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh - ``` - -2. Install Gazebo 11: - - ```bash - curl -sSL http://get.gazebosim.org | sh - ``` - - Set up your Gazebo environment variables: - - ```bash - echo "source /usr/share/gazebo/setup.sh" >> ~/.bashrc - source ~/.bashrc - ``` - -3. Install SDL2 and other dependencies: - - ```bash - sudo apt-get update && sudo apt-get install build-essential libsdl2-dev - ``` - -4. Add your user to the `input` group to enable joystick support (you need to re-login after this command): - - ```bash - sudo usermod -a -G input $USER - ``` - -5. Run the simulation: - - ```bash - make simulator - ``` - -### macOS - -1. Install Homebrew package manager, if you don't have it installed: - - ```bash - /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)" - ``` - -2. Install Arduino CLI, Gazebo 11 and SDL2: - - ```bash - brew tap osrf/simulation - brew install arduino-cli - brew install gazebo11 - brew install sdl2 - ``` - - Set up your Gazebo environment variables: - - ```bash - echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc - source ~/.zshrc - ``` - -3. Run the simulation: - - ```bash - make simulator - ``` - -### Setup and flight - -#### Control with smartphone - -1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. For **iOS**, use [QGroundControl build from TAJISOFT](https://apps.apple.com/ru/app/qgc-from-tajisoft/id1618653051). -2. Connect your smartphone to the same Wi-Fi network as the machine running the simulator. -3. If you're using a virtual machine, make sure that its network is set to the **bridged** mode with Wi-Fi adapter selected. -4. Run the simulation. -5. Open QGroundControl app. It should connect and begin showing the virtual drone's telemetry automatically. -6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. -7. Use the virtual joystick to fly the drone! - -#### Control with USB remote control - -1. Connect your USB remote control to the machine running the simulator. -2. Run the simulation. -3. Calibrate the RC using `cr` command in the command line interface. -4. Run the simulation again. -5. Use the USB remote control to fly the drone! - -## Firmware - -### Arduino IDE (Windows, Linux, macOS) - -1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended). -2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers). -3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE. -4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library): - * `FlixPeriph`, the latest version. - * `MAVLink`, version 2.0.16. -5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master). -6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE. -7. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port. -8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE. - -### Command line (Windows, Linux, macOS) - -1. [Install Arduino CLI](https://arduino.github.io/arduino-cli/installation/). - - On Linux, use: - - ```bash - curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh - ``` - -2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers). -3. Compile the firmware using `make`. Arduino dependencies will be installed automatically: - - ```bash - make - ``` - - You can flash the firmware to the board using command: - - ```bash - make upload - ``` - - You can also compile the firmware, upload it and start serial port monitoring using command: - - ```bash - make upload monitor - ``` - -See other available Make commands in the [Makefile](../Makefile). - -> [!TIP] -> You can test the firmware on a bare ESP32 board without connecting IMU and other peripherals. The Wi-Fi network `flix` should appear and all the basic functionality including CLI and QGroundControl connection should work. - -### Setup and flight - -Before flight you need to calibrate the accelerometer: - -1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line). -2. Type `ca` command there and follow the instructions. - -#### Control with smartphone - -1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. -2. Power the drone using the battery. -3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`). -4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically. -5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. -6. Use the virtual joystick to fly the drone! - -#### Control with remote control - -Before flight using remote control, you need to calibrate it: - -1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line). -2. Type `cr` command there and follow the instructions. -3. Use the remote control to fly the drone! - -#### Control with USB remote control (Wi-Fi) - -If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it. - -1. Install [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) app on your computer. -2. Connect your USB remote control to the computer. -3. Power up the drone. -4. Connect your computer to the appeared `flix` Wi-Fi network (password: `flixwifi`). -5. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically. -6. Go the the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate you USB remote control there. -7. Use the USB remote control to fly the drone! - -#### Adjusting parameters - -You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl app. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*. - - - -#### CLI access - -In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*. - - - -> [!NOTE] -> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article. - -### Firmware code structure - -See [firmware overview](firmware.md) for more details. diff --git a/docs/build.md b/docs/build.md new file mode 120000 index 0000000..d59366e --- /dev/null +++ b/docs/build.md @@ -0,0 +1 @@ +usage.md \ No newline at end of file diff --git a/docs/firmware.md b/docs/firmware.md index beeff66..d9d33d5 100644 --- a/docs/firmware.md +++ b/docs/firmware.md @@ -53,4 +53,4 @@ Armed state is stored in `armed` variable, and current mode is stored in `mode` ## Building -See build instructions in [build.md](build.md). +See build instructions in [usage.md](usage.md). diff --git a/docs/img/arming.svg b/docs/img/arming.svg new file mode 100644 index 0000000..51b5df1 --- /dev/null +++ b/docs/img/arming.svg @@ -0,0 +1,22 @@ + + + + + + + + + + diff --git a/docs/img/controls.svg b/docs/img/controls.svg new file mode 100644 index 0000000..ba1b275 --- /dev/null +++ b/docs/img/controls.svg @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + Throttle + + + + + + + Yaw + + + + + + + Pitch + + + + + + + Roll + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/img/disarming.svg b/docs/img/disarming.svg new file mode 100644 index 0000000..03b4a97 --- /dev/null +++ b/docs/img/disarming.svg @@ -0,0 +1,22 @@ + + + + + + + + + + diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index ec58888..85c0e7c 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -4,7 +4,7 @@ Do the following: -* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](build.md#firmware). +* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#firmware). * **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial. * **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*. diff --git a/docs/usage.md b/docs/usage.md new file mode 100644 index 0000000..eb4d3dc --- /dev/null +++ b/docs/usage.md @@ -0,0 +1,243 @@ +# Usage: build, setup and flight + +To use Flix, you need to build the firmware and upload it to the ESP32 board. For simulation, you need to build and run the simulator. + +For the start, clone the repository using git: + +```bash +git clone https://github.com/okalachev/flix.git +cd flix +``` + +## Simulation + +### Ubuntu + +The latest version of Ubuntu supported by Gazebo 11 simulator is 22.04. If you have a newer version, consider using a virtual machine. + +1. Install Arduino CLI: + + ```bash + curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh + ``` + +2. Install Gazebo 11: + + ```bash + curl -sSL http://get.gazebosim.org | sh + ``` + + Set up your Gazebo environment variables: + + ```bash + echo "source /usr/share/gazebo/setup.sh" >> ~/.bashrc + source ~/.bashrc + ``` + +3. Install SDL2 and other dependencies: + + ```bash + sudo apt-get update && sudo apt-get install build-essential libsdl2-dev + ``` + +4. Add your user to the `input` group to enable joystick support (you need to re-login after this command): + + ```bash + sudo usermod -a -G input $USER + ``` + +5. Run the simulation: + + ```bash + make simulator + ``` + +### macOS + +1. Install Homebrew package manager, if you don't have it installed: + + ```bash + /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)" + ``` + +2. Install Arduino CLI, Gazebo 11 and SDL2: + + ```bash + brew tap osrf/simulation + brew install arduino-cli + brew install gazebo11 + brew install sdl2 + ``` + + Set up your Gazebo environment variables: + + ```bash + echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc + source ~/.zshrc + ``` + +3. Run the simulation: + + ```bash + make simulator + ``` + +### Setup + +#### Control with smartphone + +1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. For **iOS**, use [QGroundControl build from TAJISOFT](https://apps.apple.com/ru/app/qgc-from-tajisoft/id1618653051). +2. Connect your smartphone to the same Wi-Fi network as the machine running the simulator. +3. If you're using a virtual machine, make sure that its network is set to the **bridged** mode with Wi-Fi adapter selected. +4. Run the simulation. +5. Open QGroundControl app. It should connect and begin showing the virtual drone's telemetry automatically. +6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. +7. Use the virtual joystick to fly the drone! + +#### Control with USB remote control + +1. Connect your USB remote control to the machine running the simulator. +2. Run the simulation. +3. Calibrate the RC using `cr` command in the command line interface. +4. Run the simulation again. +5. Use the USB remote control to fly the drone! + +## Firmware + +### Arduino IDE (Windows, Linux, macOS) + +1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended). +2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers). +3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE. +4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library): + * `FlixPeriph`, the latest version. + * `MAVLink`, version 2.0.16. +5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master). +6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE. +7. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port. +8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE. + +### Command line (Windows, Linux, macOS) + +1. [Install Arduino CLI](https://arduino.github.io/arduino-cli/installation/). + + On Linux, use: + + ```bash + curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh + ``` + +2. Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers). +3. Compile the firmware using `make`. Arduino dependencies will be installed automatically: + + ```bash + make + ``` + + You can flash the firmware to the board using command: + + ```bash + make upload + ``` + + You can also compile the firmware, upload it and start serial port monitoring using command: + + ```bash + make upload monitor + ``` + +See other available Make commands in the [Makefile](../Makefile). + +> [!TIP] +> You can test the firmware on a bare ESP32 board without connecting IMU and other peripherals. The Wi-Fi network `flix` should appear and all the basic functionality including CLI and QGroundControl connection should work. + +### Setup + +Before flight you need to calibrate the accelerometer: + +1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line). +2. Type `ca` command there and follow the instructions. + +#### Control with smartphone + +1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. +2. Power the drone using the battery. +3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`). +4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically. +5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. +6. Use the virtual joystick to fly the drone! + +> [!TIP] +> Decrease `TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive. + +#### Control with remote control + +Before flight using remote control, you need to calibrate it: + +1. Open Serial Monitor in Arduino IDE (or use `make monitor` command in the command line). +2. Type `cr` command there and follow the instructions. +3. Use the remote control to fly the drone! + +#### Control with USB remote control (Wi-Fi) + +If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it. + +1. Install [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) app on your computer. +2. Connect your USB remote control to the computer. +3. Power up the drone. +4. Connect your computer to the appeared `flix` Wi-Fi network (password: `flixwifi`). +5. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically. +6. Go the the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Joystick*. Calibrate you USB remote control there. +7. Use the USB remote control to fly the drone! + +> [!NOTE] +> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article. + +## Flight + +For both virtual sticks and a physical joystick, the default control scheme is left stick for throttle and yaw and right stick for pitch and roll: + + + +### Arming and disarming + +To start the motors, you should **arm** the drone. To do that, move the left stick to the bottom right corner: + + + +After that, the motors will start spinning at low speed, indicating that the drone is armed and ready to fly. + +When finished flying, **disarm** the drone, moving the left stick to the bottom left corner: + + + +### Flight modes + +Flight mode is changed using mode switch on the remote control or using the command line. + +### STAB + +The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles. + +> [!IMPORTANT] +> The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually. + +### ACRO + +In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing. + +### MANUAL + +Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**. + +## Adjusting parameters + +You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl app. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*. + + + +## CLI access + +In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*. + + diff --git a/flix/cli.ino b/flix/cli.ino index c7e6eaa..a14569a 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -14,6 +14,7 @@ extern double t; extern uint16_t channels[16]; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; extern int mode; +extern bool armed; const char* motd = "\nWelcome to\n" @@ -33,6 +34,8 @@ const char* motd = "ps - show pitch/roll/yaw\n" "psq - show attitude quaternion\n" "imu - show IMU data\n" +"arm - arm the drone\n" +"disarm - disarm the drone\n" "stab / acro - set mode\n" "rc - show RC data\n" "mot - show motor output\n" @@ -109,6 +112,10 @@ void doCommand(String str, bool echo = false) { printIMUInfo(); printIMUCalibration(); print("landed: %d\n", landed); + } else if (command == "arm") { + armed = true; + } else if (command == "disarm") { + armed = false; } else if (command == "stab") { mode = STAB; } else if (command == "acro") { @@ -121,6 +128,7 @@ void doCommand(String str, bool echo = false) { print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n", controlRoll, controlPitch, controlYaw, controlThrottle, controlMode); print("mode: %s\n", getModeName()); + print("armed: %d\n", armed); } else if (command == "mot") { print("front-right %g front-left %g rear-right %g rear-left %g\n", motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]); diff --git a/flix/control.ino b/flix/control.ino index 5bd7756..7fc1967 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -9,6 +9,7 @@ #include "lpf.h" #include "util.h" +#define ARMED_THRUST 0.1 // thrust to indicate armed state #define PITCHRATE_P 0.05 #define PITCHRATE_I 0.2 #define PITCHRATE_D 0.001 @@ -54,7 +55,7 @@ Vector torqueTarget; float thrustTarget; extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; -extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode; +extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; void control() { interpretControls(); @@ -65,14 +66,14 @@ void control() { } void interpretControls() { - armed = controlThrottle >= 0.05; - if (controlArmed < 0.5) armed = false; - // NOTE: put ACRO or MANUAL modes there if you want to use them if (controlMode < 0.25) mode = STAB; if (controlMode < 0.75) mode = STAB; if (controlMode > 0.75) mode = STAB; + if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture + if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture + thrustTarget = controlThrottle; if (mode == STAB) { @@ -137,8 +138,17 @@ void controlRates() { void controlTorque() { if (!torqueTarget.valid()) return; // skip torque control - if (!armed || thrustTarget < 0.05) { - memset(motors, 0, sizeof(motors)); // stop motors if no thrust + if (!armed) { + memset(motors, 0, sizeof(motors)); // stop motors if disarmed + return; + } + + if (thrustTarget < 0.05) { + // minimal thrust to indicate armed state + motors[0] = ARMED_THRUST; + motors[1] = ARMED_THRUST; + motors[2] = ARMED_THRUST; + motors[3] = ARMED_THRUST; return; } diff --git a/flix/failsafe.ino b/flix/failsafe.ino index ab9170b..466f091 100644 --- a/flix/failsafe.ino +++ b/flix/failsafe.ino @@ -10,19 +10,9 @@ extern double controlTime; extern float controlRoll, controlPitch, controlThrottle, controlYaw; void failsafe() { - armingFailsafe(); rcLossFailsafe(); } -// Prevent arming without zero throttle input -void armingFailsafe() { - static double zeroThrottleTime; - static double armingTime; - if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time - if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime; - if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec -} - // RC loss failsafe void rcLossFailsafe() { if (t - controlTime > RC_LOSS_TIMEOUT) { @@ -37,5 +27,8 @@ void descend() { controlPitch = 0; controlYaw = 0; controlThrottle -= dt / DESCEND_TIME; - if (controlThrottle < 0) controlThrottle = 0; + if (controlThrottle < 0) { + armed = false; + controlThrottle = 0; + } } diff --git a/flix/flix.ino b/flix/flix.ino index 7d494c9..b63582c 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -13,7 +13,7 @@ double t = NAN; // current step time, s float dt; // time delta from previous step, s float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1] -float controlArmed = NAN, controlMode = NAN; +float controlMode = NAN; Vector gyro; // gyroscope data Vector acc; // accelerometer data, m/s/s Vector rates; // filtered angular rates, rad/s diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 8f48ef0..fe1f188 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -12,11 +12,10 @@ #define PERIOD_FAST 0.1 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f -float mavlinkControlScale = 0.7; String mavlinkPrintBuffer; extern double controlTime; -extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode; +extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; void processMavlink() { sendMavlink(); @@ -99,11 +98,10 @@ void handleMavlink(const void *_msg) { if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast controlThrottle = m.z / 1000.0f; - controlPitch = m.x / 1000.0f * mavlinkControlScale; - controlRoll = m.y / 1000.0f * mavlinkControlScale; - controlYaw = m.r / 1000.0f * mavlinkControlScale; + controlPitch = m.x / 1000.0f; + controlRoll = m.y / 1000.0f; + controlYaw = m.r / 1000.0f; controlMode = NAN; - controlArmed = NAN; controlTime = t; if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0; diff --git a/flix/parameters.ino b/flix/parameters.ino index 099df2d..1f26f6b 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -70,12 +70,7 @@ Parameter parameters[] = { {"RC_PITCH", &pitchChannel}, {"RC_THROTTLE", &throttleChannel}, {"RC_YAW", &yawChannel}, - {"RC_ARMED", &armedChannel}, {"RC_MODE", &modeChannel}, -#if WIFI_ENABLED - // MAVLink - {"MAV_CTRL_SCALE", &mavlinkControlScale}, -#endif }; void setupParameters() { diff --git a/flix/rc.ino b/flix/rc.ino index 09ba957..0e001be 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -14,7 +14,7 @@ float channelZero[16]; // calibration zero values float channelMax[16]; // calibration max values // Channels mapping (using float to store in parameters): -float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, armedChannel = NAN, modeChannel = NAN; +float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN; void setupRC() { print("Setup RC\n"); @@ -42,7 +42,6 @@ void normalizeRC() { controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN; controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN; controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN; - controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : NAN; controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; } @@ -58,8 +57,7 @@ void calibrateRC() { calibrateRCChannel(&yawChannel, center, max, "5/9 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n"); calibrateRCChannel(&pitchChannel, zero, max, "6/9 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n"); calibrateRCChannel(&rollChannel, zero, max, "7/9 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n"); - calibrateRCChannel(&armedChannel, zero, max, "8/9 Switch to armed [3 sec]\n"); - calibrateRCChannel(&modeChannel, zero, max, "9/9 Disarm and switch mode to max [3 sec]\n"); + calibrateRCChannel(&modeChannel, zero, max, "9/9 Put mode switch to max [3 sec]\n"); printRCCalibration(); } @@ -94,6 +92,5 @@ void printRCCalibration() { print("Pitch %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN); print("Yaw %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN); print("Throttle %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN); - print("Armed %-7g%-7g%-7g\n", armedChannel, armedChannel >= 0 ? channelZero[(int)armedChannel] : NAN, armedChannel >= 0 ? channelMax[(int)armedChannel] : NAN); print("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN); } diff --git a/gazebo/README.md b/gazebo/README.md index 3eef4ba..c7e2608 100644 --- a/gazebo/README.md +++ b/gazebo/README.md @@ -4,7 +4,7 @@ ## Building and running -See [building and running instructions](../docs/build.md#simulation). +See [building and running instructions](../docs/usage.md#simulation). ## Code structure diff --git a/gazebo/flix.h b/gazebo/flix.h index f843597..532a5b4 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -16,7 +16,7 @@ double t = NAN; float dt; float motors[4]; float controlRoll, controlPitch, controlYaw, controlThrottle = NAN; -float controlArmed = NAN, controlMode = NAN; +float controlMode = NAN; Vector acc; Vector gyro; Vector rates; @@ -55,7 +55,6 @@ void mavlinkPrint(const char* str); void sendMavlinkPrint(); inline Quaternion fluToFrd(const Quaternion &q); void failsafe(); -void armingFailsafe(); void rcLossFailsafe(); void descend(); int parametersCount();