├── .gitmodules ├── installation.sh ├── L1AC_customization └── ArduCopter │ ├── ACRL_trajectories.h │ ├── motors.cpp │ ├── Parameters.h │ ├── config.h │ ├── ACRL_trajectories.cpp │ ├── mode.cpp │ └── Copter.h ├── README.md └── License.txt /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ardupilot"] 2 | path = ardupilot 3 | url = https://github.com/ArduPilot/ardupilot 4 | branch = Copter-4.4 5 | -------------------------------------------------------------------------------- /installation.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # Install ardupilot and the L1AC customized file pack. 4 | # This script is for Ubuntu 20.04 LTS. 5 | 6 | # Clone all the submodules 7 | git submodule update --init --recursive 8 | 9 | # Install ardupilot 10 | cd ./ardupilot/ 11 | Tools/environment_install/install-prereqs-ubuntu.sh -y 12 | . ~/.profile 13 | cd ../ 14 | 15 | # Install the L1AC customized file pack 16 | rm ./ardupilot/ArduCopter/Copter.h ./ardupilot/ArduCopter/Parameters.cpp ./ardupilot/ArduCopter/Parameters.h ./ardupilot/ArduCopter/config.h ./ardupilot/ArduCopter/mode.cpp ./ardupilot/ArduCopter/mode.h ./ardupilot/ArduCopter/motors.cpp 17 | cp ./L1AC_customization/ArduCopter/ACRL_trajectories.cpp ./L1AC_customization/ArduCopter/ACRL_trajectories.h ./L1AC_customization/ArduCopter/mode_adaptive.cpp ./L1AC_customization/ArduCopter/Copter.h ./L1AC_customization/ArduCopter/Parameters.cpp ./L1AC_customization/ArduCopter/Parameters.h ./L1AC_customization/ArduCopter/config.h ./L1AC_customization/ArduCopter/mode.cpp ./L1AC_customization/ArduCopter/mode.h ./L1AC_customization/ArduCopter/motors.cpp ./ardupilot/ArduCopter/ 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/ACRL_trajectories.h: -------------------------------------------------------------------------------- 1 | /* 2 | This program is free software: you can redistribute it and/or modify 3 | it under the terms of the GNU General Public License as published by 4 | the Free Software Foundation, either version 3 of the License, or 5 | (at your option) any later version. 6 | This program is distributed in the hope that it will be useful, 7 | but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | GNU General Public License for more details. 10 | You should have received a copy of the GNU General Public License 11 | along with this program. If not, see . 12 | */ 13 | // ACRL_trajectories.h 14 | // Copyright 2021 Sheng Cheng, all rights reserved. 15 | 16 | // Dr. Sheng Cheng, Nov. 2021 17 | // Email: chengs@illinois.edu 18 | // Advanced Controls Research Laboratory 19 | // Department of Mechanical Science and Engineering 20 | // University of Illinois Urbana-Champaign 21 | // Urbana, IL 61821, USA 22 | 23 | 24 | 25 | // #include 26 | // #include 27 | // #include 28 | // #include 29 | // #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "Copter.h" 37 | 38 | 39 | 40 | void ACRL_trajectory_takeoff(float timeInThisRun, 41 | Vector3f *targetPos, 42 | Vector3f *targetVel, 43 | Vector3f *targetAcc, 44 | Vector3f *targetJerk, 45 | Vector3f *targetSnap, 46 | Vector2f *targetYaw, 47 | Vector2f *targetYaw_dot, 48 | Vector2f *targetYaw_ddot); 49 | void ACRL_trajectory_transition_to_start(float timeInThisRun, 50 | float radiusX, 51 | float timeOffset, 52 | Vector3f *targetPos, 53 | Vector3f *targetVel, 54 | Vector3f *targetAcc, 55 | Vector3f *targetJerk, 56 | Vector3f *targetSnap, 57 | Vector2f *targetYaw, 58 | Vector2f *targetYaw_dot, 59 | Vector2f *targetYaw_ddot); 60 | void ACRL_trajectory_circle_variable_yaw(float timeInThisRun, 61 | float radius, 62 | float InitialTimeOffset, 63 | float targetSpeed, 64 | Vector3f *targetPos, 65 | Vector3f *targetVel, 66 | Vector3f *targetAcc, 67 | Vector3f *targetJerk, 68 | Vector3f *targetSnap, 69 | Vector2f *targetYaw, 70 | Vector2f *targetYaw_dot, 71 | Vector2f *targetYaw_ddot); 72 | void ACRL_trajectory_circle_fixed_yaw(float timeInThisRun, 73 | float radius, 74 | float InitialTimeOffset, 75 | float targetSpeed, 76 | Vector3f *targetPos, 77 | Vector3f *targetVel, 78 | Vector3f *targetAcc, 79 | Vector3f *targetJerk, 80 | Vector3f *targetSnap, 81 | Vector2f *targetYaw, 82 | Vector2f *targetYaw_dot, 83 | Vector2f *targetYaw_ddot); 84 | void ACRL_trajectory_figure8_fixed_yaw(float timeInThisRun, 85 | float radiusX, 86 | float radiusY, 87 | float targetSpeed, 88 | Vector3f *targetPos, 89 | Vector3f *targetVel, 90 | Vector3f *targetAcc, 91 | Vector3f *targetJerk, 92 | Vector3f *targetSnap, 93 | Vector2f *targetYaw, 94 | Vector2f *targetYaw_dot, 95 | Vector2f *targetYaw_ddot); 96 | void ACRL_trajectory_figure8_tilted(float timeInThisRun, 97 | float radiusX, 98 | float radiusY, 99 | float targetSpeed, 100 | Vector3f *targetPos, 101 | Vector3f *targetVel, 102 | Vector3f *targetAcc, 103 | Vector3f *targetJerk, 104 | Vector3f *targetSnap, 105 | Vector2f *targetYaw, 106 | Vector2f *targetYaw_dot, 107 | Vector2f *targetYaw_ddot); 108 | uint8_t ACRL_trajectory_land(float currentTime, 109 | Vector3f currentPosition, 110 | Vector3f currentVelocity, 111 | float currentYaw, 112 | float decRate, 113 | Vector3f *targetPos, 114 | Vector3f *targetVel, 115 | Vector3f *targetAcc, 116 | Vector3f *targetJerk, 117 | Vector3f *targetSnap, 118 | Vector2f *targetYaw, 119 | Vector2f *targetYaw_dot, 120 | Vector2f *targetYaw_ddot); 121 | 122 | float polyEval(float polyCoef[],float x,int N); 123 | float polyDiffEval(float polyDiffCoef[],float x,int N); 124 | float polyDiff2Eval(float polyDiffDiffCoef[],float x,int N); 125 | float polyDiff3Eval(float polyDiffDiffCoef[],float x,int N); 126 | float polyDiff4Eval(float polyDiffDiffCoef[],float x,int N); 127 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # L1Quad 2 | This project contains an implementation of the L1 Adaptive Control (L1AC) for quadrotors based on ardupilot. The details on the algorithm and experimental results can be found in the following paper: 3 | * [L1Quad: L1 Adaptive Augmentation of Geometric Control for Agile Quadrotors with Performance Guarantees](https://arxiv.org/abs/2302.07208). 4 | 5 | The demo can be accessed [here](https://youtu.be/18-2OqTRJ50?si=T6rJvlOqwevCKzZk). 6 | 7 | ## Installation 8 | Follow the steps below to clone this repository and install it on your computer. It has been tested on Ubuntu 20.04. You can alternatively set up the Ardupilot development environment on your own following [ArduPilot Setup in Linux](https://ardupilot.org/dev/docs/building-setup-linux.html#building-setup-linux). 9 | 10 | ```bash 11 | # clone this repository 12 | git clone https://github.com/sigma-pi/L1Quad 13 | cd L1Quad 14 | 15 | # make the bash script executable and then install 16 | chmod +x installation.sh 17 | ./installation.sh 18 | ``` 19 | 20 | ## Usage 21 | Below, we show the usage of L1Quad in Ardupilot's software-in-the-loop (SITL) simulation. 22 | 23 | ```bash 24 | # for SITL, first make sure REAL_OR_SITL in ardupilot/ArduCopter/config.h is 0 25 | # under L1Quad, go to autotest and run the simulation 26 | cd ./ardupilot/Tools/autotest/ 27 | python3 sim_vehicle.py --console -A "--uartF=sim:vicon:" --map -v ArduCopter -f X 28 | ``` 29 | After the console shows "prearm good", you should see an icon of quadcopter on the map. Check the parameters and settings in the same terminal (as the one running python script above). We will command the quadrotor to fly a figure8 trajectory. 30 | ```bash 31 | # set the figure8 trajectory (2 m long and 1 m wide, with a maximum linear speed of 2 m/s) before the flight 32 | param set TRAJINDEX 3 # figure8 trajectory 33 | param set CIRCRADIUSX 2 # m 34 | param set CIRCRADIUSY 1 # m 35 | param set CIRCSPEED 2 # m/s 36 | 37 | # LANDFLAG must be 0 before the flight. It would also be better to set L1 off (L1ENABLE = 0) before the flight 38 | param set LANDFLAG 0 39 | param set L1ENABLE 0 40 | ``` 41 | Now you can arm the quadrotor and then enter the adaptive mode (mode 29). You need to arm the quadrotor before entering mode 29: it will immediately take off and gradually accelerate to 2 m/s while flying the figure8 trajectory. 42 | ```bash 43 | arm throttle 44 | rc 3 1200 45 | mode 29 46 | ``` 47 | 48 | You can turn L1 on by setting L1ENABLE to 1 at any time during the flight by 49 | ```bash 50 | param set L1ENABLE 1 # this will turn L1 on 51 | ``` 52 | 53 | You can land the quadrotor with the following command: 54 | ```bash 55 | param set LANDFLAG 1 56 | ``` 57 | 58 | ## Fly L1Quad on a real quadrotor 59 | 60 | Please contact [Sheng Cheng](https://github.com/Sheng-Cheng) if you want to use it for flights with a real quadrotor: You may need to tune the controller gains for flight tests with real quadrotors. 61 | 62 | You will follow the Ardupilot's firmware compile procedure and load the custom firmware to your Pixhawk (Pixhawk4-mini, CubeOrange, CubeOrangePlus have been tested). 63 | ```bash 64 | # under ardupilot, first make sure REAL_OR_SITL in ardupilot/ArduCopter/config.h is 1 65 | ./waf configure --board CubeOrangePlus 66 | ./waf copter 67 | ``` 68 | 69 | ## Details on configuration and settings 70 | We implemented L1Quad as a custom mode (29, Mode Adaptive) following the [instructions by Ardupilot](https://ardupilot.org/dev/docs/apmcopter-adding-a-new-flight-mode.html). For our implementation, we use the [geometric controller](https://ieeexplore.ieee.org/document/5717652) as the baseline control for trajectory tracking. 71 | 72 | Below we list relevant parameters that you can modify. You may need to tune the controller gains for flight tests with real quadrotors. 73 | ```bash 74 | # L1 adaptive controller 75 | ASV # As for translational dynamics in L1's state predictor 76 | ASOMEGA # As for the rotational dynamics in L1's state predictor 77 | CTOFFQ1THRUST # LPF1's cutoff frequency for thrust channel 78 | CTOFFQ1MOMENT # LPF1's cutoff frequency for moment channel 79 | CTOFFQ2MOMENT # LPF2's cutoff frequency for moment channel 80 | L1ENABLE # enable switch for L1 adaptive controller 81 | 82 | # Geometric controller 83 | # position P term 84 | GEOCTRL_KPX # kpx for geometric controller 85 | GEOCTRL_KPY # kpy for geometric controller 86 | GEOCTRL_KPZ # kpz for geometric controller 87 | # position D term 88 | GEOCTRL_KVX # kvx for geometric controller 89 | GEOCTRL_KVY # kvy for geometric controller 90 | GEOCTRL_KVZ # kvz for geometric controller 91 | # angular P term 92 | GEOCTRL_KRX # kRx for geometric controller 93 | GEOCTRL_KRY # kRy for geometric controller 94 | GEOCTRL_KRZ # kRz for geometric controller 95 | # angular D term 96 | GEOCTRL_KOX # kOmegax for geometric controller 97 | GEOCTRL_KOY # kOmegay for geometric controller 98 | GEOCTRL_KOZ # kOmegaz for geometric controller 99 | 100 | # Other parameters 101 | CIRCSPEED # speed of a circular trajectory (maximum speed of a figure8 trajectory) 102 | CIRCRADIUSX # circle radius or figure8's x radius (length) 103 | CIRCRADIUSY # figure8's y radius (width) (Note: this parameter is not used for a circular trajectory) 104 | TRAJINDEX # index of the trajectory to fly (see the list below) 105 | LANDFLAG # landing control signal. Setting it to 1 to switch to landing operation and it will disable takeoff. Make sure to set it to 0 before the flight. 106 | ``` 107 | 108 | List of trajectories: 109 | ```bash 110 | # TRAJINDEX default value is 0, which will be default hover at 1 m. 111 | # You need to specify the trajectory before the flight 112 | param set TRAJINDEX 1 # variable yaw circular 113 | param set TRAJINDEX 2 # fixed yaw circular 114 | param set TRAJINDEX 3 # fixed yaw figure8 115 | param set TRAJINDEX 4 # fixed yaw figure8 with tilted altitude 116 | ``` 117 | 118 | **Note**: 119 | In ardupilot/ArduCopter/config.h, **we added a flag REAL_OR_SITL to tell the compiler whether to use the settings for real drone tests or SITL simulations.** Set it to 0 (default) for SITL, and 1 for compiling firmware for a Pixhawk. Check this value before compiling. 120 | 121 | ## License 122 | Please read the license attached to this repository. 123 | 124 | ## Citation 125 | If this repository is helpful for your project, please cite the following papers: 126 | ```bash 127 | @inproceedings{wu20221L1adaptive, 128 | title={L1 adaptive augmentation for geometric tracking control of quadrotors}, 129 | author={Wu, Zhuohuan and Cheng, Sheng and Ackerman, Kasey A and Gahlawat, Aditya and Lakshmanan, Arun and Zhao, Pan and Hovakimyan, Naira}, 130 | booktitle={2022 International Conference on Robotics and Automation (ICRA)}, 131 | pages={1329--1336}, 132 | year={2022}, 133 | organization={IEEE} 134 | } 135 | @article{wu2025l1quad, 136 | title={{L1Quad}: L1 Adaptive Augmentation of Geometric Control for Agile Quadrotors With Performance Guarantees}, 137 | author={Wu, Zhuohuan and Cheng, Sheng and Zhao, Pan and Gahlawat, Aditya and Ackerman, Kasey A and Lakshmanan, Arun and Yang, Chengyu and Yu, Jiahao and Hovakimyan, Naira}, 138 | journal={IEEE Transactions on Control Systems Technology}, 139 | volume={33}, 140 | number={2}, 141 | pages={597--612}, 142 | year={2025}, 143 | publisher={IEEE} 144 | } 145 | ``` 146 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/motors.cpp: -------------------------------------------------------------------------------- 1 | #include "Copter.h" 2 | 3 | #define ARM_DELAY 20 // called at 10hz so 2 seconds 4 | #define DISARM_DELAY 20 // called at 10hz so 2 seconds 5 | #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds 6 | #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second 7 | 8 | static uint32_t auto_disarm_begin; 9 | 10 | // arm_motors_check - checks for pilot input to arm or disarm the copter 11 | // called at 10hz 12 | void Copter::arm_motors_check() 13 | { 14 | static int16_t arming_counter; 15 | 16 | // check if arming/disarm using rudder is allowed 17 | AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type(); 18 | if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) { 19 | return; 20 | } 21 | 22 | #if TOY_MODE_ENABLED == ENABLED 23 | if (g2.toy_mode.enabled()) { 24 | // not armed with sticks in toy mode 25 | return; 26 | } 27 | #endif 28 | 29 | // ensure throttle is down 30 | if (channel_throttle->get_control_in() > 0) { 31 | arming_counter = 0; 32 | return; 33 | } 34 | 35 | int16_t yaw_in = channel_yaw->get_control_in(); 36 | 37 | // full right 38 | if (yaw_in > 4000) { 39 | 40 | // increase the arming counter to a maximum of 1 beyond the auto trim counter 41 | if (arming_counter <= AUTO_TRIM_DELAY) { 42 | arming_counter++; 43 | } 44 | 45 | // arm the motors and configure for flight 46 | if (arming_counter == ARM_DELAY && !motors->armed()) { 47 | // reset arming counter if arming fail 48 | if (!arming.arm(AP_Arming::Method::RUDDER)) { 49 | arming_counter = 0; 50 | } 51 | } 52 | 53 | // arm the motors and configure for flight 54 | if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && flightmode->mode_number() == Mode::Number::STABILIZE) { 55 | gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); 56 | auto_trim_counter = 250; 57 | auto_trim_started = false; 58 | // ensure auto-disarm doesn't trigger immediately 59 | auto_disarm_begin = millis(); 60 | } 61 | 62 | // full left and rudder disarming is enabled 63 | } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { 64 | if (!flightmode->has_manual_throttle() && !ap.land_complete) { 65 | arming_counter = 0; 66 | return; 67 | } 68 | 69 | // increase the counter to a maximum of 1 beyond the disarm delay 70 | if (arming_counter <= DISARM_DELAY) { 71 | arming_counter++; 72 | } 73 | 74 | // disarm the motors 75 | if (arming_counter == DISARM_DELAY && motors->armed()) { 76 | arming.disarm(AP_Arming::Method::RUDDER); 77 | } 78 | 79 | // Yaw is centered so reset arming counter 80 | } else { 81 | arming_counter = 0; 82 | } 83 | } 84 | 85 | // auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds 86 | void Copter::auto_disarm_check() 87 | { 88 | uint32_t tnow_ms = millis(); 89 | uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127); 90 | 91 | // exit immediately if we are already disarmed, or if auto 92 | // disarming is disabled 93 | if (!motors->armed() || disarm_delay_ms == 0 || flightmode->mode_number() == Mode::Number::THROW) { 94 | auto_disarm_begin = tnow_ms; 95 | return; 96 | } 97 | 98 | // if the rotor is still spinning, don't initiate auto disarm 99 | if (motors->get_spool_state() > AP_Motors::SpoolState::GROUND_IDLE) { 100 | auto_disarm_begin = tnow_ms; 101 | return; 102 | } 103 | 104 | // always allow auto disarm if using interlock switch or motors are Emergency Stopped 105 | if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) { 106 | #if FRAME_CONFIG != HELI_FRAME 107 | // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less 108 | // obvious the copter is armed as the motors will not be spinning 109 | disarm_delay_ms /= 2; 110 | #endif 111 | } else { 112 | bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0; 113 | bool thr_low; 114 | if (flightmode->has_manual_throttle() || !sprung_throttle_stick) { 115 | thr_low = ap.throttle_zero; 116 | } else { 117 | float deadband_top = get_throttle_mid() + g.throttle_deadzone; 118 | thr_low = channel_throttle->get_control_in() <= deadband_top; 119 | } 120 | 121 | if (!thr_low || !ap.land_complete) { 122 | // reset timer 123 | auto_disarm_begin = tnow_ms; 124 | } 125 | } 126 | 127 | // disarm once timer expires 128 | if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) { 129 | arming.disarm(AP_Arming::Method::DISARMDELAY); 130 | auto_disarm_begin = tnow_ms; 131 | } 132 | } 133 | 134 | // motors_output - send output to motors library which will adjust and send to ESCs and servos 135 | void Copter::motors_output() 136 | { 137 | #if ADVANCED_FAILSAFE == ENABLED 138 | // this is to allow the failsafe module to deliberately crash 139 | // the vehicle. Only used in extreme circumstances to meet the 140 | // OBC rules 141 | if (g2.afs.should_crash_vehicle()) { 142 | g2.afs.terminate_vehicle(); 143 | if (!g2.afs.terminating_vehicle_via_landing()) { 144 | return; 145 | } 146 | // landing must continue to run the motors output 147 | } 148 | #endif 149 | 150 | // Update arming delay state 151 | if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || flightmode->mode_number() == Mode::Number::THROW)) { 152 | ap.in_arming_delay = false; 153 | } 154 | 155 | // output any servo channels 156 | SRV_Channels::calc_pwm(); 157 | 158 | // cork now, so that all channel outputs happen at once 159 | SRV_Channels::cork(); 160 | 161 | // update output on any aux channels, for manual passthru 162 | SRV_Channels::output_ch_all(); 163 | 164 | // update motors interlock state 165 | bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); 166 | if (!motors->get_interlock() && interlock) { 167 | motors->set_interlock(true); 168 | AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED); 169 | } else if (motors->get_interlock() && !interlock) { 170 | motors->set_interlock(false); 171 | AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED); 172 | } 173 | 174 | if (ap.motor_test) { 175 | // check if we are performing the motor test 176 | motor_test_output(); 177 | } else if (copter.flightmode->mode_number() != Mode::Number::ADAPTIVE){ 178 | // send output signals to motors 179 | flightmode->output_to_motors(); 180 | } 181 | 182 | // push all channels 183 | SRV_Channels::push(); 184 | } 185 | 186 | // check for pilot stick input to trigger lost vehicle alarm 187 | void Copter::lost_vehicle_check() 188 | { 189 | static uint8_t soundalarm_counter; 190 | 191 | // disable if aux switch is setup to vehicle alarm as the two could interfere 192 | if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOST_VEHICLE_SOUND)) { 193 | return; 194 | } 195 | 196 | // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch 197 | if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) { 198 | if (soundalarm_counter >= LOST_VEHICLE_DELAY) { 199 | if (AP_Notify::flags.vehicle_lost == false) { 200 | AP_Notify::flags.vehicle_lost = true; 201 | gcs().send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm"); 202 | } 203 | } else { 204 | soundalarm_counter++; 205 | } 206 | } else { 207 | soundalarm_counter = 0; 208 | if (AP_Notify::flags.vehicle_lost == true) { 209 | AP_Notify::flags.vehicle_lost = false; 210 | } 211 | } 212 | } 213 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | NON-EXCLUSIVE RESEARCH USE LICENSE 2 | FOR L1Quad SOFTWARE 3 | 4 | 5 | The Software (as defined below) will only be licensed to You (as defined below) upon the condition that You accept all of the terms and conditions contained in this license agreement (“License”). Please read this License carefully. By downloading and using the Software, You accept the terms and conditions of this License. 6 | 7 | 8 | 1. Definitions. 9 | 1.1. “Commercial Use” means sale, lease, license, distribution or otherwise making the Software available to a third party. 10 | 1.2. “Derivative Work” means any derivative work, as defined by U.S. copyright law, of the Software, including any modification, enhancement, upgrade, or improvement to the Software. 11 | 1.3. “License” means this document. 12 | 1.4. “Software” means the L1Quad software in source code format, Technology No. TF09094, “Adaptive control for uncertain nonlinear multi-input multi-output systems”. Software implements the algorithms covered under patent # 8,712,559. 13 | 1.5. “Research Use” means use of the Software for Your non-profit research, development, academic, educational or personal and individual use, and expressly excludes Commercial Use. 14 | 1.6. “You” (or “Your”) means an individual or legal entity exercising rights under, and complying with all of the terms of, this License. For an individual, “You” includes your personal use of the Software only. For a legal entity, “You” includes all entities under common control with the entity and all employees of the entity. For purposes of this License, “common control” means right to control, or actual control of, the management of such other entity, whether by ownership of securities, voting rights, management authority, agreement, or otherwise. 15 | 16 | 17 | 2. License Grant and Restrictions. 18 | Subject to Your compliance with the terms and conditions of this License: 19 | 2.1. License Grant: The Board of Trustees of the University of Illinois (“ILLINOIS”), on behalf of the Department of Mechanical Science and Engineering, grants You a non-exclusive, non-transferable, restricted license to: 20 | a. Use the Software for Your Research Use; and 21 | b. Create Derivative Works of the Software for Your Research Use. 22 | 2.2. Restrictions: 23 | a. You shall not make Commercial Use of the Software or Derivative Works of the Software without having executed an appropriate Commercial Use license with ILLINOIS. Contact ILLINOIS at otm@illinois.edu to determine whether such a Commercial Use license is available. 24 | b. You may make Derivative Works only as allowed under the terms of this License agreement. 25 | c. You shall not combine the Software or Derivative Works with any other software under terms requiring that such combination, or any portion of the Software or Derivative Works, be made available other than as allowed under the terms of this License agreement. 26 | 2.3. You acknowledge the Software is owned by ILLINOIS, and ILLINOIS retains all right, title, and interest in and to the Software. 27 | 2.4. Nothing in this License shall be construed as conferring any license under any of ILLINOIS’ or any third party’s intellectual property rights, whether by estoppel, implication, or otherwise. 28 | 2.5. You shall clearly mark and rename all Derivative Works of the Software to notify users that it is a modified version of the Software. 29 | 2.6. You shall reproduce the following notice on all Derivative Works: “This software uses or is derived from the L1Quad software developed by the Department of Mechanical Science and Engineering at the University of Illinois Urbana-Champaign.” 30 | 2.7. For any reports or published results obtained using the Software or Derivative Works of the Software, You shall acknowledge use of the Software by the following citation: 31 | a. “The L1Quad Software, used by [insert Your name], was developed by the Department of Mechanical Science and Engineering at the University of Illinois Urbana-Champaign.” 32 | b. You must supply one copy of each such report or publication to ILLINOIS through Naira Hovakimyan at nhovakim@illinois.edu. 33 | 34 | 35 | 3. Confidential Information. 36 | 3.1. You acknowledge that the Software is proprietary to ILLINOIS. You agree to protect the Software from unauthorized disclosure, use, or release and to treat the Software with at least the same level of care as You use to protect Your own proprietary software and/or confidential information, but in no event no less than a reasonable standard of care. 37 | 3.2. If You become aware of any unauthorized licensing, copying, or use of the Software, You shall promptly notify ILLINOIS in writing at otm@illinois.edu. 38 | 3.3. You agree to use the Software only in the manner and for the specific uses authorized in this License. 39 | 40 | 41 | 4. Feedback/Contributions 42 | 4.1. Because the Software is being distributed free of charge pursuant to this License, ILLINOIS encourages You to provide feedback on the Software. ILLINOIS also encourages You to provide information regarding your experience with using the Software for your research. Please submit such feedback to Naira Hovakimyan at nhovakim@illinois.edu. 43 | 4.2. ILLINOIS encourages contributions from users of the Software that might be used or incorporated by ILLINOIS, in its sole discretion, into future versions or revisions of the Software. By contributing code, You agree that such code may be used and distributed by ILLINOIS for any purpose. 44 | 45 | 46 | 5. Limitation of Warranties. 47 | 5.1. EXCEPT AS SET FORTH IN THIS LICENSE, TO THE FULLEST EXTENT ALLOWED BY APPLICABLE LAW, THE SOFTWARE IS PROVIDED “AS IS,” AND ILLINOIS DISCLAIMS ALL EXPRESS AND IMPLIED CONDITIONS, REPRESENTATIONS AND WARRANTIES, INCLUDING WITHOUT LIMITATION ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT OF ANY PATENT, COPYRIGHT, TRADEMARK OR OTHER RIGHTS OF THIRD PARTIES IN CONNECTION WITH THE USE OF THE SOFTWARE BY ANY PERSON OR ENTITY. 48 | 5.2. THIS LIMITATION OF WARRANTY IS AN ESSENTIAL PART OF THIS LICENSE. NO USE OF THE SOFTWARE IS AUTHORIZED EXCEPT UNDER THIS DISCLAIMER. 49 | 5.3. ANY SOFTWARE DOWNLOADED IS AT YOUR OWN DISCRETION AND RISK, AND YOU ARE SOLELY RESPONSIBLE FOR ANY DAMAGE TO YOUR COMPUTER SYSTEM OR LOSS OF DATA THAT RESULTS FROM THE DOWNLOAD OF SUCH SOFTWARE, INCLUDING ANY DAMAGES RESULTING FROM COMPUTER VIRUSES. 50 | 51 | 52 | 6. Indemnification. 53 | You shall indemnify, defend, and hold harmless ILLINOIS (including its trustees, fellows, officers, employees, students, and agents) against any and all claims, losses, damages, and/or liability of whatsoever nature, as well as costs and expenses (including reasonable attorneys’ fees) arising out of or related to Your use, or inability to use, the Software or Derivative Works of the Software, regardless of theory of liability, whether for breach or in tort (including negligence). 54 | 55 | 56 | 7. Limitation of Liability. 57 | 7.1. You assume the entire risk as to the quality, results, performance, and/or non-performance of the Software and all Derivative Works of the Software. Should the Software or Derivative Works of the Software prove defective in any manner, You assume the entire cost of any necessary, servicing, repair or correction. 58 | 7.2. In no event shall ILLINOIS (including its trustees, fellows, officers, employees, students, and agents) be responsible or liable for any direct, indirect, special, incidental, punitive, consequential or other damages whatsoever (including lost profits, business, revenue, use, data, or other economic advantage) in connection with, or arising out of, or related to this License, regardless of the theory of liability, whether for breach or in tort (including negligence), even if ILLINOIS may have been previously advised of the possibility of such damage. 59 | 7.3. Liability for damages shall be limited and excluded as set forth in this Section, even if any remedy provided for in this License fails of its essential purpose. 60 | 61 | 62 | 8. Termination. 63 | 8.1. This License is effective until December 31, 2033, unless terminated earlier as provided herein. 64 | 8.2. You may terminate this License at any time by destroying all copies of the Software and Derivative Works of the Software. 65 | 8.3. This License, and the rights granted hereunder, will terminate automatically, and without any further notice from or action by ILLINOIS, if You fail to comply with any obligation of this License. 66 | 8.4. Upon termination, You must immediately cease use of and destroy all copies of the Software and Derivative Works of the Software and verify such destruction in writing to ILLINOIS. 67 | 8.5. All provisions of this License that by their nature contemplate rights and obligations of the parties to be enjoyed or performed after the termination of the License will survive termination of all, or any part of the License, including the following: Sections 3, 5, 6, 7, 9, and 10. 68 | 69 | 70 | 9. Export Controls. 71 | The Software delivered under this License and any Derivative Works of the Software may be subject to U.S. export control laws and may be subject to export or import regulations in other countries. You agree to comply strictly with all such applicable laws and regulations and acknowledge that You have the responsibility, at Your own expense, to obtain such licenses to export, re-export, or import as may be required. 72 | 73 | 74 | 10. Miscellaneous. 75 | 10.1. Governing Law. This License shall be governed by the laws of the State of Illinois, U.S.A., without regard to any conflict of laws provisions. 76 | 10.2. Severability. If any provision(s) of this License are held to be invalid, illegal, or unenforceable by a court of competent jurisdiction, such invalidity or unenforceability shall not in any way affect the validity or enforceability of the remaining provisions. 77 | 10.3. Assignment. You may not assign or otherwise transfer any of Your rights or obligations under this License, without the prior written consent of ILLINOIS. 78 | 10.4. Entire Agreement. This License represents the parties’ entire agreement relating to the Software. Except as otherwise provided herein, no modification of this License is binding unless in writing and signed by an authorized representative of each party. 79 | 10.5. Waiver. The failure of either party to enforce any provision of this License shall not constitute a waiver of that right or future enforcement of that or any other provision. The waiver of a breach or default of this License may be effected only in writing by the waiving party. 80 | 81 | 82 | Approved for legal form, Office of University Counsel, DS 04/2022 83 | UIUC Technology No. TF09094 Page of Non-Exclusive Research Use Software License -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/Parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define AP_PARAM_VEHICLE_NAME copter 4 | 5 | #include 6 | #include "RC_Channel.h" 7 | #include 8 | 9 | #include 10 | #if AP_GRIPPER_ENABLED 11 | # include 12 | #endif 13 | #if MODE_FOLLOW_ENABLED == ENABLED 14 | # include 15 | #endif 16 | #if WEATHERVANE_ENABLED == ENABLED 17 | #include 18 | #endif 19 | 20 | // Global parameter class. 21 | // 22 | class Parameters { 23 | public: 24 | // The version of the layout as described by the parameter enum. 25 | // 26 | // When changing the parameter enum in an incompatible fashion, this 27 | // value should be incremented by one. 28 | // 29 | // The increment will prevent old parameters from being used incorrectly 30 | // by newer code. 31 | // 32 | static const uint16_t k_format_version = 120; 33 | 34 | // Parameter identities. 35 | // 36 | // The enumeration defined here is used to ensure that every parameter 37 | // or parameter group has a unique ID number. This number is used by 38 | // AP_Param to store and locate parameters in EEPROM. 39 | // 40 | // Note that entries without a number are assigned the next number after 41 | // the entry preceding them. When adding new entries, ensure that they 42 | // don't overlap. 43 | // 44 | // Try to group related variables together, and assign them a set 45 | // range in the enumeration. Place these groups in numerical order 46 | // at the end of the enumeration. 47 | // 48 | // WARNING: Care should be taken when editing this enumeration as the 49 | // AP_Param load/save code depends on the values here to identify 50 | // variables saved in EEPROM. 51 | // 52 | // 53 | enum { 54 | // Layout version number, always key zero. 55 | // 56 | k_param_format_version = 0, 57 | k_param_software_type, // deprecated 58 | k_param_ins_old, // *** Deprecated, remove with next eeprom number change 59 | k_param_ins, // libraries/AP_InertialSensor variables 60 | k_param_NavEKF2_old, // deprecated - remove 61 | k_param_NavEKF2, 62 | k_param_g2, // 2nd block of parameters 63 | k_param_NavEKF3, 64 | k_param_can_mgr, 65 | k_param_osd, 66 | 67 | // simulation 68 | k_param_sitl = 10, 69 | 70 | // barometer object (needed for SITL) 71 | k_param_barometer, 72 | 73 | // scheduler object (for debugging) 74 | k_param_scheduler, 75 | 76 | // relay object 77 | k_param_relay, 78 | 79 | // (old) EPM object 80 | k_param_epm_unused, 81 | 82 | // BoardConfig object 83 | k_param_BoardConfig, 84 | 85 | // GPS object 86 | k_param_gps, 87 | 88 | // Parachute object 89 | k_param_parachute, 90 | 91 | // Landing gear object 92 | k_param_landinggear, // 18 93 | 94 | // Input Management object 95 | k_param_input_manager, // 19 96 | 97 | // Misc 98 | // 99 | k_param_log_bitmask_old = 20, // Deprecated 100 | k_param_log_last_filenumber, // *** Deprecated - remove 101 | // with next eeprom number 102 | // change 103 | k_param_toy_yaw_rate, // deprecated - remove 104 | k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change 105 | k_param_rssi_pin, // unused, replaced by rssi_ library parameters 106 | k_param_throttle_accel_enabled, // deprecated - remove 107 | k_param_wp_yaw_behavior, 108 | k_param_acro_trainer, 109 | k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max 110 | k_param_circle_rate, // deprecated - remove 111 | k_param_rangefinder_gain, // deprecated - remove 112 | k_param_ch8_option_old, // deprecated 113 | k_param_arming_check_old, // deprecated - remove 114 | k_param_sprayer, 115 | k_param_angle_max, 116 | k_param_gps_hdop_good, 117 | k_param_battery, 118 | k_param_fs_batt_mah, // unused - moved to AP_BattMonitor 119 | k_param_angle_rate_max, // remove 120 | k_param_rssi_range, // unused, replaced by rssi_ library parameters 121 | k_param_rc_feel_rp, // deprecated 122 | k_param_NavEKF, // deprecated - remove 123 | k_param_mission, // mission library 124 | k_param_rc_13_old, 125 | k_param_rc_14_old, 126 | k_param_rally, 127 | k_param_poshold_brake_rate, 128 | k_param_poshold_brake_angle_max, 129 | k_param_pilot_accel_z, 130 | k_param_serial0_baud, // deprecated - remove 131 | k_param_serial1_baud, // deprecated - remove 132 | k_param_serial2_baud, // deprecated - remove 133 | k_param_land_repositioning, 134 | k_param_rangefinder, // rangefinder object 135 | k_param_fs_ekf_thresh, 136 | k_param_terrain, 137 | k_param_acro_rp_expo, // deprecated - remove 138 | k_param_throttle_deadzone, 139 | k_param_optflow, 140 | k_param_dcmcheck_thresh, // deprecated - remove 141 | k_param_log_bitmask, 142 | k_param_cli_enabled_old, // deprecated - remove 143 | k_param_throttle_filt, 144 | k_param_throttle_behavior, 145 | k_param_pilot_takeoff_alt, // 64 146 | 147 | // 65: AP_Limits Library 148 | k_param_limits = 65, // deprecated - remove 149 | k_param_gpslock_limit, // deprecated - remove 150 | k_param_geofence_limit, // deprecated - remove 151 | k_param_altitude_limit, // deprecated - remove 152 | k_param_fence_old, // only used for conversion 153 | k_param_gps_glitch, // deprecated 154 | k_param_baro_glitch, // 71 - deprecated 155 | 156 | // AP_ADSB Library 157 | k_param_adsb, // 72 158 | k_param_notify, // 73 159 | 160 | // 74: precision landing object 161 | k_param_precland = 74, 162 | 163 | // 164 | // 75: Singlecopter, CoaxCopter 165 | // 166 | k_param_single_servo_1 = 75, // remove 167 | k_param_single_servo_2, // remove 168 | k_param_single_servo_3, // remove 169 | k_param_single_servo_4, // 78 - remove 170 | 171 | // 172 | // 80: Heli 173 | // 174 | k_param_heli_servo_1 = 80, // remove 175 | k_param_heli_servo_2, // remove 176 | k_param_heli_servo_3, // remove 177 | k_param_heli_servo_4, // remove 178 | k_param_heli_pitch_ff, // remove 179 | k_param_heli_roll_ff, // remove 180 | k_param_heli_yaw_ff, // remove 181 | k_param_heli_stab_col_min, // remove 182 | k_param_heli_stab_col_max, // remove 183 | k_param_heli_servo_rsc, // 89 = full! - remove 184 | 185 | // 186 | // 90: misc2 187 | // 188 | k_param_motors = 90, 189 | k_param_disarm_delay, 190 | k_param_fs_crash_check, 191 | k_param_throw_motor_start, 192 | k_param_rtl_alt_type, 193 | k_param_avoid, 194 | k_param_avoidance_adsb, 195 | 196 | // 97: RSSI 197 | k_param_rssi = 97, 198 | 199 | // 200 | // 100: Inertial Nav 201 | // 202 | k_param_inertial_nav = 100, // deprecated 203 | k_param_wp_nav, 204 | k_param_attitude_control, 205 | k_param_pos_control, 206 | k_param_circle_nav, 207 | k_param_loiter_nav, // 105 208 | k_param_custom_control, 209 | 210 | // 110: Telemetry control 211 | // 212 | k_param_gcs0 = 110, 213 | k_param_gcs1, 214 | k_param_sysid_this_mav, 215 | k_param_sysid_my_gcs, 216 | k_param_serial1_baud_old, // deprecated 217 | k_param_telem_delay, 218 | k_param_gcs2, 219 | k_param_serial2_baud_old, // deprecated 220 | k_param_serial2_protocol, // deprecated 221 | k_param_serial_manager, 222 | k_param_ch9_option_old, 223 | k_param_ch10_option_old, 224 | k_param_ch11_option_old, 225 | k_param_ch12_option_old, 226 | k_param_takeoff_trigger_dz_old, 227 | k_param_gcs3, 228 | k_param_gcs_pid_mask, // 126 229 | k_param_gcs4, 230 | k_param_gcs5, 231 | k_param_gcs6, 232 | 233 | // 234 | // 135 : reserved for Solo until features merged with master 235 | // 236 | k_param_rtl_speed_cms = 135, 237 | k_param_fs_batt_curr_rtl, 238 | k_param_rtl_cone_slope, // 137 239 | 240 | // 241 | // 140: Sensor parameters 242 | // 243 | k_param_imu = 140, // deprecated - can be deleted 244 | k_param_battery_monitoring = 141, // deprecated - can be deleted 245 | k_param_volt_div_ratio, // deprecated - can be deleted 246 | k_param_curr_amp_per_volt, // deprecated - can be deleted 247 | k_param_input_voltage, // deprecated - can be deleted 248 | k_param_pack_capacity, // deprecated - can be deleted 249 | k_param_compass_enabled_deprecated, 250 | k_param_compass, 251 | k_param_rangefinder_enabled_old, // deprecated 252 | k_param_frame_type, 253 | k_param_optflow_enabled, // deprecated 254 | k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor 255 | k_param_ch7_option_old, 256 | k_param_auto_slew_rate, // deprecated - can be deleted 257 | k_param_rangefinder_type_old, // deprecated 258 | k_param_super_simple = 155, 259 | k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change 260 | k_param_copter_leds_mode, // deprecated - remove with next eeprom number change 261 | k_param_ahrs, // AHRS group // 159 262 | 263 | // 264 | // 160: Navigation parameters 265 | // 266 | k_param_rtl_altitude = 160, 267 | k_param_crosstrack_gain, // deprecated - remove with next eeprom number change 268 | k_param_rtl_loiter_time, 269 | k_param_rtl_alt_final, 270 | k_param_tilt_comp, // 164 deprecated - remove with next eeprom number change 271 | 272 | 273 | // 274 | // Camera and mount parameters 275 | // 276 | k_param_camera = 165, 277 | k_param_camera_mount, 278 | k_param_camera_mount2, // deprecated 279 | 280 | // 281 | // Battery monitoring parameters 282 | // 283 | k_param_battery_volt_pin = 168, // deprecated - can be deleted 284 | k_param_battery_curr_pin, // 169 deprecated - can be deleted 285 | 286 | // 287 | // 170: Radio settings 288 | // 289 | k_param_rc_1_old = 170, 290 | k_param_rc_2_old, 291 | k_param_rc_3_old, 292 | k_param_rc_4_old, 293 | k_param_rc_5_old, 294 | k_param_rc_6_old, 295 | k_param_rc_7_old, 296 | k_param_rc_8_old, 297 | k_param_rc_10_old, 298 | k_param_rc_11_old, 299 | k_param_throttle_min, // remove 300 | k_param_throttle_max, // remove 301 | k_param_failsafe_throttle, 302 | k_param_throttle_fs_action, // remove 303 | k_param_failsafe_throttle_value, 304 | k_param_throttle_trim, // remove 305 | k_param_esc_calibrate, 306 | k_param_radio_tuning, 307 | k_param_radio_tuning_high_old, // unused 308 | k_param_radio_tuning_low_old, // unused 309 | k_param_rc_speed = 192, 310 | k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor 311 | k_param_throttle_mid, // remove 312 | k_param_failsafe_gps_enabled, // remove 313 | k_param_rc_9_old, 314 | k_param_rc_12_old, 315 | k_param_failsafe_gcs, 316 | k_param_rcmap, // 199 317 | 318 | // 319 | // 200: flight modes 320 | // 321 | k_param_flight_mode1 = 200, 322 | k_param_flight_mode2, 323 | k_param_flight_mode3, 324 | k_param_flight_mode4, 325 | k_param_flight_mode5, 326 | k_param_flight_mode6, 327 | k_param_simple_modes, 328 | k_param_flight_mode_chan, 329 | k_param_initial_mode, 330 | 331 | // 332 | // 210: Waypoint data 333 | // 334 | k_param_waypoint_mode = 210, // remove 335 | k_param_command_total, // remove 336 | k_param_command_index, // remove 337 | k_param_command_nav_index, // remove 338 | k_param_waypoint_radius, // remove 339 | k_param_circle_radius, // remove 340 | k_param_waypoint_speed_max, // remove 341 | k_param_land_speed, 342 | k_param_auto_velocity_z_min, // remove 343 | k_param_auto_velocity_z_max, // remove - 219 344 | k_param_land_speed_high, 345 | 346 | // 347 | // 220: PI/D Controllers 348 | // 349 | k_param_acro_rp_p = 221, // remove 350 | k_param_axis_lock_p, // remove 351 | k_param_pid_rate_roll, // remove 352 | k_param_pid_rate_pitch, // remove 353 | k_param_pid_rate_yaw, // remove 354 | k_param_p_stabilize_roll, // remove 355 | k_param_p_stabilize_pitch, // remove 356 | k_param_p_stabilize_yaw, // remove 357 | k_param_p_pos_xy, // remove 358 | k_param_p_loiter_lon, // remove 359 | k_param_pid_loiter_rate_lat, // remove 360 | k_param_pid_loiter_rate_lon, // remove 361 | k_param_pid_nav_lat, // remove 362 | k_param_pid_nav_lon, // remove 363 | k_param_p_alt_hold, // remove 364 | k_param_p_vel_z, // remove 365 | k_param_pid_optflow_roll, // remove 366 | k_param_pid_optflow_pitch, // remove 367 | k_param_acro_balance_roll_old, // remove 368 | k_param_acro_balance_pitch_old, // remove 369 | k_param_pid_accel_z, // remove 370 | k_param_acro_balance_roll, 371 | k_param_acro_balance_pitch, 372 | k_param_acro_yaw_p, // remove 373 | k_param_autotune_axis_bitmask, // remove 374 | k_param_autotune_aggressiveness, // remove 375 | k_param_pi_vel_xy, // remove 376 | k_param_fs_ekf_action, 377 | k_param_rtl_climb_min, 378 | k_param_rpm_sensor, 379 | k_param_autotune_min_d, // remove 380 | k_param_arming, // 252 - AP_Arming 381 | k_param_logger = 253, // 253 - Logging Group 382 | 383 | // 254,255: reserved 384 | 385 | k_param_vehicle = 257, // vehicle common block of parameters 386 | 387 | // ACRL: L1 adaptive controller 388 | k_param_Asv = 258, // As for the velocity state 389 | k_param_Asomega, // As for the rotational velocity state 390 | k_param_ctoffq1Thrust, // LPF1's cutoff frequency for thrust channel 391 | k_param_ctoffq1Moment, // LPF1's cutoff frequency for moment channel 392 | k_param_ctoffq2Moment, // LPF2's cutoff frequency for moment channel 393 | k_param_circSpeed, // circulating speed of the circular trajectory 394 | k_param_l1enable = 264, // enable switch for L1 adaptive controller 395 | 396 | // ACRL: Geometric controller 397 | // position P term 398 | k_param_GeoCtrl_Kpx = 265, // kpx for geometric controller 399 | k_param_GeoCtrl_Kpy, // kpy for geometric controller 400 | k_param_GeoCtrl_Kpz, // kpz for geometric controller 401 | // position D term 402 | k_param_GeoCtrl_Kvx, // kvx for geometric controller 403 | k_param_GeoCtrl_Kvy, // kvy for geometric controller 404 | k_param_GeoCtrl_Kvz, // kvz for geometric controller 405 | // angular P term 406 | k_param_GeoCtrl_KRx, // kRx for geometric controller 407 | k_param_GeoCtrl_KRy, // kRy for geometric controller 408 | k_param_GeoCtrl_KRz, // kRz for geometric controller 409 | // angular D term 410 | k_param_GeoCtrl_KOx, // kOmegax for geometric controller 411 | k_param_GeoCtrl_KOy, // kOmegay for geometric controller 412 | k_param_GeoCtrl_KOz = 276, // kOmegaz for geometric controller 413 | 414 | // ACRL: Other parameters 415 | k_param_circRadiusX = 277, // circle radius or figure8's x radius 416 | k_param_circRadiusY, // figure8's y radius (not used for circle radius) 417 | k_param_trajIndex, // index of the trajectory to run 418 | k_param_LandFlag = 280, // flag of landing 419 | 420 | // the k_param_* space is 9-bits in size 421 | // 511: reserved 422 | }; 423 | 424 | AP_Int16 format_version; 425 | 426 | // Telemetry control 427 | // 428 | AP_Int16 sysid_this_mav; 429 | AP_Int16 sysid_my_gcs; 430 | AP_Int8 telem_delay; 431 | 432 | AP_Float throttle_filt; 433 | AP_Int16 throttle_behavior; 434 | AP_Float pilot_takeoff_alt; 435 | 436 | #if MODE_RTL_ENABLED == ENABLED 437 | AP_Int32 rtl_altitude; 438 | AP_Int16 rtl_speed_cms; 439 | AP_Float rtl_cone_slope; 440 | AP_Int16 rtl_alt_final; 441 | AP_Int16 rtl_climb_min; // rtl minimum climb in cm 442 | AP_Int32 rtl_loiter_time; 443 | AP_Enum rtl_alt_type; 444 | #endif 445 | 446 | AP_Int8 failsafe_gcs; // ground station failsafe behavior 447 | AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position 448 | 449 | AP_Int8 super_simple; 450 | 451 | AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions 452 | 453 | #if MODE_POSHOLD_ENABLED == ENABLED 454 | AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec 455 | AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees 456 | #endif 457 | 458 | // Waypoints 459 | // 460 | AP_Int16 land_speed; 461 | AP_Int16 land_speed_high; 462 | AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request 463 | AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request 464 | 465 | // Throttle 466 | // 467 | AP_Int8 failsafe_throttle; 468 | AP_Int16 failsafe_throttle_value; 469 | AP_Int16 throttle_deadzone; 470 | 471 | // Flight modes 472 | // 473 | AP_Int8 flight_mode1; 474 | AP_Int8 flight_mode2; 475 | AP_Int8 flight_mode3; 476 | AP_Int8 flight_mode4; 477 | AP_Int8 flight_mode5; 478 | AP_Int8 flight_mode6; 479 | AP_Int8 simple_modes; 480 | AP_Int8 flight_mode_chan; 481 | AP_Int8 initial_mode; 482 | 483 | // Misc 484 | // 485 | AP_Int32 log_bitmask; 486 | AP_Int8 esc_calibrate; 487 | AP_Int8 radio_tuning; 488 | AP_Int8 frame_type; 489 | AP_Int8 disarm_delay; 490 | 491 | AP_Int8 land_repositioning; 492 | AP_Int8 fs_ekf_action; 493 | AP_Int8 fs_crash_check; 494 | AP_Float fs_ekf_thresh; 495 | AP_Int16 gcs_pid_mask; 496 | 497 | // ACRL 498 | // L1 adaptive controller 499 | AP_Float Asv; // As for the velocity state 500 | AP_Float Asomega; // As for the rotational velocity state 501 | AP_Float ctoffq1Thrust; // LPF1's cutoff frequency for thrust channel 502 | AP_Float ctoffq1Moment; // LPF1's cutoff frequency for moment channel 503 | AP_Float ctoffq2Moment; // LPF2's cutoff frequency for moment channel 504 | AP_Float circSpeed; // speed for the circular trajectory 505 | AP_Int8 l1enable; // enabling switch for L1 adaptive controller 506 | 507 | // Geometric controller 508 | AP_Float GeoCtrl_Kpx; // kpx for geometric controller 509 | AP_Float GeoCtrl_Kpy; // kpy for geometric controller 510 | AP_Float GeoCtrl_Kpz; // kpz for geometric controllerF 511 | AP_Float GeoCtrl_Kvx; // kvx for geometric controller 512 | AP_Float GeoCtrl_Kvy; // kvy for geometric controller 513 | AP_Float GeoCtrl_Kvz; // kvz for geometric controllerF 514 | AP_Float GeoCtrl_KRx; // kRx for geometric controller 515 | AP_Float GeoCtrl_KRy; // kRy for geometric controller 516 | AP_Float GeoCtrl_KRz; // kRz for geometric controllerF 517 | AP_Float GeoCtrl_KOx; // kOmegax for geometric controller 518 | AP_Float GeoCtrl_KOy; // kOmegay for geometric controller 519 | AP_Float GeoCtrl_KOz; // kOmegaz for geometric controller 520 | 521 | // Other parameters 522 | AP_Float circRadiusX; // circle radius or figure8's x radius 523 | AP_Float circRadiusY; // figure8's y radius (not used for circle radius) 524 | AP_Int8 trajIndex; // index of the trajectory to run 525 | AP_Int8 LandFlag; // flag of landing 526 | 527 | #if MODE_THROW_ENABLED == ENABLED 528 | AP_Enum throw_motor_start; 529 | #endif 530 | 531 | AP_Int16 rc_speed; // speed of fast RC Channels in Hz 532 | 533 | #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED 534 | // Acro parameters 535 | AP_Float acro_balance_roll; 536 | AP_Float acro_balance_pitch; 537 | #endif 538 | 539 | #if MODE_ACRO_ENABLED == ENABLED 540 | // Acro parameters 541 | AP_Int8 acro_trainer; 542 | #endif 543 | 544 | // Note: keep initializers here in the same order as they are declared 545 | // above. 546 | Parameters() 547 | { 548 | } 549 | }; 550 | 551 | /* 552 | 2nd block of parameters, to avoid going past 256 top level keys 553 | */ 554 | class ParametersG2 { 555 | public: 556 | ParametersG2(void); 557 | 558 | // var_info for holding Parameter information 559 | static const struct AP_Param::GroupInfo var_info[]; 560 | static const struct AP_Param::GroupInfo var_info2[]; 561 | 562 | // altitude at which nav control can start in takeoff 563 | AP_Float wp_navalt_min; 564 | 565 | // button checking 566 | #if HAL_BUTTON_ENABLED 567 | AP_Button *button_ptr; 568 | #endif 569 | 570 | #if STATS_ENABLED == ENABLED 571 | // vehicle statistics 572 | AP_Stats stats; 573 | #endif 574 | 575 | #if AP_GRIPPER_ENABLED 576 | AP_Gripper gripper; 577 | #endif 578 | 579 | #if MODE_THROW_ENABLED == ENABLED 580 | // Throw mode parameters 581 | AP_Int8 throw_nextmode; 582 | AP_Enum throw_type; 583 | #endif 584 | 585 | // ground effect compensation enable/disable 586 | AP_Int8 gndeffect_comp_enabled; 587 | 588 | // temperature calibration handling 589 | AP_TempCalibration temp_calibration; 590 | 591 | #if BEACON_ENABLED == ENABLED 592 | // beacon (non-GPS positioning) library 593 | AP_Beacon beacon; 594 | #endif 595 | 596 | #if HAL_PROXIMITY_ENABLED 597 | // proximity (aka object avoidance) library 598 | AP_Proximity proximity; 599 | #endif 600 | 601 | // whether to enforce acceptance of packets only from sysid_my_gcs 602 | AP_Int8 sysid_enforce; 603 | 604 | #if ADVANCED_FAILSAFE == ENABLED 605 | // advanced failsafe library 606 | AP_AdvancedFailsafe_Copter afs; 607 | #endif 608 | 609 | // developer options 610 | AP_Int32 dev_options; 611 | 612 | #if MODE_ACRO_ENABLED == ENABLED 613 | AP_Float acro_thr_mid; 614 | #endif 615 | 616 | // frame class 617 | AP_Int8 frame_class; 618 | 619 | // RC input channels 620 | RC_Channels_Copter rc_channels; 621 | 622 | // control over servo output ranges 623 | SRV_Channels servo_channels; 624 | 625 | #if MODE_SMARTRTL_ENABLED == ENABLED 626 | // Safe RTL library 627 | AP_SmartRTL smart_rtl; 628 | #endif 629 | 630 | // wheel encoder and winch 631 | #if AP_WINCH_ENABLED 632 | AP_Winch winch; 633 | #endif 634 | 635 | // Additional pilot velocity items 636 | AP_Int16 pilot_speed_dn; 637 | 638 | // Land alt final stage 639 | AP_Int16 land_alt_low; 640 | 641 | #if TOY_MODE_ENABLED == ENABLED 642 | ToyMode toy_mode; 643 | #endif 644 | 645 | #if MODE_FLOWHOLD_ENABLED 646 | // we need a pointer to the mode for the G2 table 647 | void *mode_flowhold_ptr; 648 | #endif 649 | 650 | #if MODE_FOLLOW_ENABLED == ENABLED 651 | // follow 652 | AP_Follow follow; 653 | #endif 654 | 655 | #ifdef USER_PARAMS_ENABLED 656 | // User custom parameters 657 | UserParameters user_parameters; 658 | #endif 659 | 660 | #if AUTOTUNE_ENABLED == ENABLED 661 | // we need a pointer to autotune for the G2 table 662 | void *autotune_ptr; 663 | #endif 664 | 665 | #if AP_SCRIPTING_ENABLED 666 | AP_Scripting scripting; 667 | #endif // AP_SCRIPTING_ENABLED 668 | 669 | AP_Float tuning_min; 670 | AP_Float tuning_max; 671 | 672 | #if AC_OAPATHPLANNER_ENABLED == ENABLED 673 | // object avoidance path planning 674 | AP_OAPathPlanner oa; 675 | #endif 676 | 677 | #if MODE_SYSTEMID_ENABLED == ENABLED 678 | // we need a pointer to the mode for the G2 table 679 | void *mode_systemid_ptr; 680 | #endif 681 | 682 | // vibration failsafe enable/disable 683 | AP_Int8 fs_vibe_enabled; 684 | 685 | // Failsafe options bitmask #36 686 | AP_Int32 fs_options; 687 | 688 | #if MODE_AUTOROTATE_ENABLED == ENABLED 689 | // Autonmous autorotation 690 | AC_Autorotation arot; 691 | #endif 692 | 693 | #if MODE_ZIGZAG_ENABLED == ENABLED 694 | // we need a pointer to the mode for the G2 table 695 | void *mode_zigzag_ptr; 696 | #endif 697 | 698 | // command model parameters 699 | #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED 700 | AC_CommandModel command_model_acro_rp; 701 | #endif 702 | 703 | #if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED 704 | AC_CommandModel command_model_acro_y; 705 | #endif 706 | 707 | AC_CommandModel command_model_pilot; 708 | 709 | #if MODE_ACRO_ENABLED == ENABLED 710 | AP_Int8 acro_options; 711 | #endif 712 | 713 | #if MODE_AUTO_ENABLED == ENABLED 714 | AP_Int32 auto_options; 715 | #endif 716 | 717 | #if MODE_GUIDED_ENABLED == ENABLED 718 | AP_Int32 guided_options; 719 | #endif 720 | 721 | AP_Float fs_gcs_timeout; 722 | 723 | #if MODE_RTL_ENABLED == ENABLED 724 | AP_Int32 rtl_options; 725 | #endif 726 | 727 | AP_Int32 flight_options; 728 | 729 | #if RANGEFINDER_ENABLED == ENABLED 730 | AP_Float rangefinder_filt; 731 | #endif 732 | 733 | #if MODE_GUIDED_ENABLED == ENABLED 734 | AP_Float guided_timeout; 735 | #endif 736 | 737 | AP_Int8 surftrak_mode; 738 | AP_Int8 failsafe_dr_enable; 739 | AP_Int16 failsafe_dr_timeout; 740 | AP_Float surftrak_tc; 741 | 742 | // ramp time of throttle during take-off 743 | AP_Float takeoff_throttle_slew_time; 744 | #if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME 745 | AP_Int16 takeoff_rpm_min; 746 | #endif 747 | 748 | #if WEATHERVANE_ENABLED == ENABLED 749 | AC_WeatherVane weathervane; 750 | #endif 751 | 752 | // payload place parameters 753 | AP_Float pldp_thrust_placed_fraction; 754 | AP_Float pldp_range_finder_minimum_m; 755 | AP_Float pldp_delay_s; 756 | AP_Float pldp_descent_speed_ms; 757 | }; 758 | 759 | extern const AP_Param::Info var_info[]; 760 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/config.h: -------------------------------------------------------------------------------- 1 | // 2 | #pragma once 3 | 4 | ////////////////////////////////////////////////////////////////////////////// 5 | ////////////////////////////////////////////////////////////////////////////// 6 | // 7 | // WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING 8 | // 9 | // DO NOT EDIT this file to adjust your configuration. Create your own 10 | // APM_Config.h and use APM_Config.h.example as a reference. 11 | // 12 | // WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING 13 | /// 14 | ////////////////////////////////////////////////////////////////////////////// 15 | ////////////////////////////////////////////////////////////////////////////// 16 | // 17 | // Default and automatic configuration details. 18 | // 19 | // Notes for maintainers: 20 | // 21 | // - Try to keep this file organised in the same order as APM_Config.h.example 22 | // 23 | #include "defines.h" 24 | 25 | /// 26 | /// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that 27 | /// change in your local copy of APM_Config.h. 28 | /// 29 | #include "APM_Config.h" 30 | 31 | 32 | ////////////////////////////////////////////////////////////////////////////// 33 | ////////////////////////////////////////////////////////////////////////////// 34 | // HARDWARE CONFIGURATION AND CONNECTIONS 35 | ////////////////////////////////////////////////////////////////////////////// 36 | ////////////////////////////////////////////////////////////////////////////// 37 | 38 | #ifndef CONFIG_HAL_BOARD 39 | #error CONFIG_HAL_BOARD must be defined to build ArduCopter 40 | #endif 41 | 42 | #ifndef ARMING_DELAY_SEC 43 | # define ARMING_DELAY_SEC 2.0f 44 | #endif 45 | 46 | ////////////////////////////////////////////////////////////////////////////// 47 | // FRAME_CONFIG 48 | // 49 | #ifndef FRAME_CONFIG 50 | # define FRAME_CONFIG MULTICOPTER_FRAME 51 | #endif 52 | 53 | ///////////////////////////////////////////////////////////////////////////////// 54 | // TradHeli defaults 55 | #if FRAME_CONFIG == HELI_FRAME 56 | # define RC_FAST_SPEED 125 57 | # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AHEAD 58 | #endif 59 | 60 | ////////////////////////////////////////////////////////////////////////////// 61 | // PWM control 62 | // default RC speed in Hz 63 | #ifndef RC_FAST_SPEED 64 | # define RC_FAST_SPEED 490 65 | #endif 66 | 67 | ////////////////////////////////////////////////////////////////////////////// 68 | // Rangefinder 69 | // 70 | 71 | #ifndef RANGEFINDER_ENABLED 72 | # define RANGEFINDER_ENABLED ENABLED 73 | #endif 74 | 75 | #ifndef RANGEFINDER_HEALTH_MAX 76 | # define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder 77 | #endif 78 | 79 | #ifndef RANGEFINDER_TIMEOUT_MS 80 | # define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second 81 | #endif 82 | 83 | #ifndef RANGEFINDER_FILT_DEFAULT 84 | # define RANGEFINDER_FILT_DEFAULT 0.5f // filter for rangefinder distance 85 | #endif 86 | 87 | #ifndef SURFACE_TRACKING_TIMEOUT_MS 88 | # define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt 89 | #endif 90 | 91 | #ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF 92 | # define RANGEFINDER_TILT_CORRECTION ENABLED 93 | #endif 94 | 95 | #ifndef RANGEFINDER_GLITCH_ALT_CM 96 | # define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch 97 | #endif 98 | 99 | #ifndef RANGEFINDER_GLITCH_NUM_SAMPLES 100 | # define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading 101 | #endif 102 | 103 | #ifndef MAV_SYSTEM_ID 104 | # define MAV_SYSTEM_ID 1 105 | #endif 106 | 107 | // prearm GPS hdop check 108 | #ifndef GPS_HDOP_GOOD_DEFAULT 109 | # define GPS_HDOP_GOOD_DEFAULT 140 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled 110 | #endif 111 | 112 | // missing terrain data failsafe 113 | #ifndef FS_TERRAIN_TIMEOUT_MS 114 | #define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL) 115 | #endif 116 | 117 | // pre-arm baro vs inertial nav max alt disparity 118 | #ifndef PREARM_MAX_ALT_DISPARITY_CM 119 | # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters 120 | #endif 121 | 122 | ////////////////////////////////////////////////////////////////////////////// 123 | // EKF Failsafe 124 | #ifndef FS_EKF_ACTION_DEFAULT 125 | # define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_LAND // EKF failsafe triggers land by default 126 | #endif 127 | #ifndef FS_EKF_THRESHOLD_DEFAULT 128 | # define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered 129 | #endif 130 | 131 | #ifndef EKF_ORIGIN_MAX_ALT_KM 132 | # define EKF_ORIGIN_MAX_ALT_KM 50 // EKF origin and home must be within 50km vertically 133 | #endif 134 | 135 | ////////////////////////////////////////////////////////////////////////////// 136 | // Auto Tuning 137 | #ifndef AUTOTUNE_ENABLED 138 | # define AUTOTUNE_ENABLED ENABLED 139 | #endif 140 | 141 | ////////////////////////////////////////////////////////////////////////////// 142 | // Precision Landing with companion computer or IRLock sensor 143 | #ifndef PRECISION_LANDING 144 | # define PRECISION_LANDING ENABLED 145 | #endif 146 | 147 | ////////////////////////////////////////////////////////////////////////////// 148 | // Parachute release 149 | #ifndef PARACHUTE 150 | # define PARACHUTE HAL_PARACHUTE_ENABLED 151 | #endif 152 | 153 | ////////////////////////////////////////////////////////////////////////////// 154 | // Nav-Guided - allows external nav computer to control vehicle 155 | #ifndef NAV_GUIDED 156 | # define NAV_GUIDED !HAL_MINIMIZE_FEATURES 157 | #endif 158 | 159 | ////////////////////////////////////////////////////////////////////////////// 160 | // Acro - fly vehicle in acrobatic mode 161 | #ifndef MODE_ACRO_ENABLED 162 | # define MODE_ACRO_ENABLED ENABLED 163 | #endif 164 | 165 | ////////////////////////////////////////////////////////////////////////////// 166 | // Auto mode - allows vehicle to trace waypoints and perform automated actions 167 | #ifndef MODE_AUTO_ENABLED 168 | # define MODE_AUTO_ENABLED ENABLED 169 | #endif 170 | 171 | ////////////////////////////////////////////////////////////////////////////// 172 | // Brake mode - bring vehicle to stop 173 | #ifndef MODE_BRAKE_ENABLED 174 | # define MODE_BRAKE_ENABLED ENABLED 175 | #endif 176 | 177 | ////////////////////////////////////////////////////////////////////////////// 178 | // Circle - fly vehicle around a central point 179 | #ifndef MODE_CIRCLE_ENABLED 180 | # define MODE_CIRCLE_ENABLED ENABLED 181 | #endif 182 | 183 | ////////////////////////////////////////////////////////////////////////////// 184 | // Drift - fly vehicle in altitude-held, coordinated-turn mode 185 | #ifndef MODE_DRIFT_ENABLED 186 | # define MODE_DRIFT_ENABLED ENABLED 187 | #endif 188 | 189 | ////////////////////////////////////////////////////////////////////////////// 190 | // flip - fly vehicle in flip in pitch and roll direction mode 191 | #ifndef MODE_FLIP_ENABLED 192 | # define MODE_FLIP_ENABLED ENABLED 193 | #endif 194 | 195 | ////////////////////////////////////////////////////////////////////////////// 196 | // Follow - follow another vehicle or GCS 197 | #ifndef MODE_FOLLOW_ENABLED 198 | # define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES 199 | #endif 200 | 201 | ////////////////////////////////////////////////////////////////////////////// 202 | // Guided mode - control vehicle's position or angles from GCS 203 | #ifndef MODE_GUIDED_ENABLED 204 | # define MODE_GUIDED_ENABLED ENABLED 205 | #endif 206 | 207 | ////////////////////////////////////////////////////////////////////////////// 208 | // GuidedNoGPS mode - control vehicle's angles from GCS 209 | #ifndef MODE_GUIDED_NOGPS_ENABLED 210 | # define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES 211 | #endif 212 | 213 | ////////////////////////////////////////////////////////////////////////////// 214 | // Loiter mode - allows vehicle to hold global position 215 | #ifndef MODE_LOITER_ENABLED 216 | # define MODE_LOITER_ENABLED ENABLED 217 | #endif 218 | 219 | ////////////////////////////////////////////////////////////////////////////// 220 | // Position Hold - enable holding of global position 221 | #ifndef MODE_POSHOLD_ENABLED 222 | # define MODE_POSHOLD_ENABLED ENABLED 223 | #endif 224 | 225 | ////////////////////////////////////////////////////////////////////////////// 226 | // RTL - Return To Launch 227 | #ifndef MODE_RTL_ENABLED 228 | # define MODE_RTL_ENABLED ENABLED 229 | #endif 230 | 231 | ////////////////////////////////////////////////////////////////////////////// 232 | // SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home 233 | #ifndef MODE_SMARTRTL_ENABLED 234 | # define MODE_SMARTRTL_ENABLED ENABLED 235 | #endif 236 | 237 | ////////////////////////////////////////////////////////////////////////////// 238 | // Sport - fly vehicle in rate-controlled (earth-frame) mode 239 | #ifndef MODE_SPORT_ENABLED 240 | # define MODE_SPORT_ENABLED DISABLED 241 | #endif 242 | 243 | ////////////////////////////////////////////////////////////////////////////// 244 | // System ID - conduct system identification tests on vehicle 245 | #ifndef MODE_SYSTEMID_ENABLED 246 | # define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES 247 | #endif 248 | 249 | ////////////////////////////////////////////////////////////////////////////// 250 | // Throw - fly vehicle after throwing it in the air 251 | #ifndef MODE_THROW_ENABLED 252 | # define MODE_THROW_ENABLED ENABLED 253 | #endif 254 | 255 | ////////////////////////////////////////////////////////////////////////////// 256 | // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B 257 | #ifndef MODE_ZIGZAG_ENABLED 258 | # define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES 259 | #endif 260 | 261 | ////////////////////////////////////////////////////////////////////////////// 262 | // Turtle - allow vehicle to be flipped over after a crash 263 | #ifndef MODE_TURTLE_ENABLED 264 | # define MODE_TURTLE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(DISABLE_DSHOT) && FRAME_CONFIG != HELI_FRAME 265 | #endif 266 | 267 | ////////////////////////////////////////////////////////////////////////////// 268 | // Adaptive mode - allow vehicle to run adaptive controller 269 | #ifndef MODE_ADAPTIVE_ENABLED 270 | # define MODE_ADAPTIVE_ENABLED ENABLED 271 | #endif 272 | 273 | #ifndef REAL_OR_SITL 274 | # define REAL_OR_SITL 0 // 0 for SITL and 1 for REAL 275 | #endif 276 | 277 | #if !REAL_OR_SITL 278 | // default ACRL controller parameters (SITL) 279 | #ifndef GEOCTRL_KPX_DEFAULT 280 | #define GEOCTRL_KPX_DEFAULT 18.0f 281 | #endif 282 | 283 | #ifndef GEOCTRL_KPY_DEFAULT 284 | #define GEOCTRL_KPY_DEFAULT 18.0f 285 | #endif 286 | 287 | #ifndef GEOCTRL_KPZ_DEFAULT 288 | #define GEOCTRL_KPZ_DEFAULT 27.6f 289 | #endif 290 | 291 | #ifndef GEOCTRL_KVX_DEFAULT 292 | #define GEOCTRL_KVX_DEFAULT 4.0f 293 | #endif 294 | 295 | #ifndef GEOCTRL_KVY_DEFAULT 296 | #define GEOCTRL_KVY_DEFAULT 4.0f 297 | #endif 298 | 299 | #ifndef GEOCTRL_KVZ_DEFAULT 300 | #define GEOCTRL_KVZ_DEFAULT 6.0f 301 | #endif 302 | 303 | #ifndef GEOCTRL_KRX_DEFAULT 304 | #define GEOCTRL_KRX_DEFAULT 5.4f 305 | #endif 306 | 307 | #ifndef GEOCTRL_KRY_DEFAULT 308 | #define GEOCTRL_KRY_DEFAULT 5.4f 309 | #endif 310 | 311 | #ifndef GEOCTRL_KRZ_DEFAULT 312 | #define GEOCTRL_KRZ_DEFAULT 0.092f 313 | #endif 314 | 315 | #ifndef GEOCTRL_KOX_DEFAULT 316 | #define GEOCTRL_KOX_DEFAULT 0.6f 317 | #endif 318 | 319 | #ifndef GEOCTRL_KOY_DEFAULT 320 | #define GEOCTRL_KOY_DEFAULT 0.6f 321 | #endif 322 | 323 | #ifndef GEOCTRL_KOZ_DEFAULT 324 | #define GEOCTRL_KOZ_DEFAULT 0.023f 325 | #endif 326 | 327 | #ifndef L1ENABLE_DEFAULT 328 | #define L1ENABLE_DEFAULT 0 329 | #endif 330 | 331 | #ifndef ASV_DEFAULT 332 | #define ASV_DEFAULT -5.0 333 | #endif 334 | 335 | #ifndef ASOMEGA_DEFAULT 336 | #define ASOMEGA_DEFAULT -10.0 337 | #endif 338 | 339 | #ifndef CTOFFQ1THRUST_DEFAULT 340 | #define CTOFFQ1THRUST_DEFAULT 10.0 341 | #endif 342 | 343 | #ifndef CTOFFQ1MOMENT_DEFAULT 344 | #define CTOFFQ1MOMENT_DEFAULT 10.0 345 | #endif 346 | 347 | #ifndef CTOFFQ2MOMENT_DEFAULT 348 | #define CTOFFQ2MOMENT_DEFAULT 2.0 349 | #endif 350 | 351 | #ifndef CIRCSPEED_DEFAULT 352 | #define CIRCSPEED_DEFAULT 0.3 353 | #endif 354 | 355 | #ifndef CIRCRADIUSX_DEFAULT 356 | #define CIRCRADIUSX_DEFAULT 2.0 357 | #endif 358 | 359 | #ifndef CIRCRADIUSY_DEFAULT 360 | #define CIRCRADIUSY_DEFAULT 1.0 361 | #endif 362 | 363 | #ifndef TRAJINDEX_DEFAULT 364 | #define TRAJINDEX_DEFAULT 0.0 365 | #endif 366 | 367 | #ifndef LANDFLAG_DEFAULT 368 | #define LANDFLAG_DEFAULT 0.0 369 | #endif 370 | 371 | #elif REAL_OR_SITL 372 | // default ACRL controller parameters (real) 373 | #ifndef GEOCTRL_KPX_DEFAULT 374 | #define GEOCTRL_KPX_DEFAULT 14.0f 375 | #endif 376 | 377 | #ifndef GEOCTRL_KPY_DEFAULT 378 | #define GEOCTRL_KPY_DEFAULT 15.0f 379 | #endif 380 | 381 | #ifndef GEOCTRL_KPZ_DEFAULT 382 | #define GEOCTRL_KPZ_DEFAULT 15.0f 383 | #endif 384 | 385 | #ifndef GEOCTRL_KVX_DEFAULT 386 | #define GEOCTRL_KVX_DEFAULT 1.5f 387 | #endif 388 | 389 | #ifndef GEOCTRL_KVY_DEFAULT 390 | #define GEOCTRL_KVY_DEFAULT 0.9f 391 | #endif 392 | 393 | #ifndef GEOCTRL_KVZ_DEFAULT 394 | #define GEOCTRL_KVZ_DEFAULT 1.1f 395 | #endif 396 | 397 | #ifndef GEOCTRL_KRX_DEFAULT 398 | #define GEOCTRL_KRX_DEFAULT 0.55f 399 | #endif 400 | 401 | #ifndef GEOCTRL_KRY_DEFAULT 402 | #define GEOCTRL_KRY_DEFAULT 0.35f 403 | #endif 404 | 405 | #ifndef GEOCTRL_KRZ_DEFAULT 406 | #define GEOCTRL_KRZ_DEFAULT 0.15f 407 | #endif 408 | 409 | #ifndef GEOCTRL_KOX_DEFAULT 410 | #define GEOCTRL_KOX_DEFAULT 0.035f 411 | #endif 412 | 413 | #ifndef GEOCTRL_KOY_DEFAULT 414 | #define GEOCTRL_KOY_DEFAULT 0.03f 415 | #endif 416 | 417 | #ifndef GEOCTRL_KOZ_DEFAULT 418 | #define GEOCTRL_KOZ_DEFAULT 0.004f 419 | #endif 420 | 421 | #ifndef L1ENABLE_DEFAULT 422 | #define L1ENABLE_DEFAULT 0 423 | #endif 424 | 425 | #ifndef ASV_DEFAULT 426 | #define ASV_DEFAULT -5.0 427 | #endif 428 | 429 | #ifndef ASOMEGA_DEFAULT 430 | #define ASOMEGA_DEFAULT -10.0 431 | #endif 432 | 433 | #ifndef CTOFFQ1THRUST_DEFAULT 434 | #define CTOFFQ1THRUST_DEFAULT 30.0 435 | #endif 436 | 437 | #ifndef CTOFFQ1MOMENT_DEFAULT 438 | #define CTOFFQ1MOMENT_DEFAULT 5.0 439 | #endif 440 | 441 | #ifndef CTOFFQ2MOMENT_DEFAULT 442 | #define CTOFFQ2MOMENT_DEFAULT 15.0 443 | #endif 444 | 445 | #ifndef CIRCSPEED_DEFAULT 446 | #define CIRCSPEED_DEFAULT 0.0 447 | #endif 448 | 449 | #ifndef CIRCRADIUSX_DEFAULT 450 | #define CIRCRADIUSX_DEFAULT 2.0 451 | #endif 452 | 453 | #ifndef CIRCRADIUSY_DEFAULT 454 | #define CIRCRADIUSY_DEFAULT 1.0 455 | #endif 456 | 457 | #ifndef TRAJINDEX_DEFAULT 458 | #define TRAJINDEX_DEFAULT 0.0 459 | #endif 460 | 461 | #ifndef LANDFLAG_DEFAULT 462 | #define LANDFLAG_DEFAULT 0.0 463 | #endif 464 | #endif 465 | 466 | ////////////////////////////////////////////////////////////////////////////// 467 | // Flowhold - use optical flow to hover in place 468 | #ifndef MODE_FLOWHOLD_ENABLED 469 | # define MODE_FLOWHOLD_ENABLED !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED 470 | #endif 471 | 472 | ////////////////////////////////////////////////////////////////////////////// 473 | 474 | ////////////////////////////////////////////////////////////////////////////// 475 | // Weathervane - allow vehicle to yaw into wind 476 | #ifndef WEATHERVANE_ENABLED 477 | # define WEATHERVANE_ENABLED !HAL_MINIMIZE_FEATURES 478 | #endif 479 | 480 | ////////////////////////////////////////////////////////////////////////////// 481 | 482 | ////////////////////////////////////////////////////////////////////////////// 483 | // Autorotate - autonomous auto-rotation - helicopters only 484 | #if CONFIG_HAL_BOARD == HAL_BOARD_SITL 485 | #if FRAME_CONFIG == HELI_FRAME 486 | #ifndef MODE_AUTOROTATE_ENABLED 487 | # define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES 488 | #endif 489 | #else 490 | # define MODE_AUTOROTATE_ENABLED DISABLED 491 | #endif 492 | #else 493 | # define MODE_AUTOROTATE_ENABLED DISABLED 494 | #endif 495 | 496 | ////////////////////////////////////////////////////////////////////////////// 497 | 498 | // Beacon support - support for local positioning systems 499 | #ifndef BEACON_ENABLED 500 | # define BEACON_ENABLED !HAL_MINIMIZE_FEATURES 501 | #endif 502 | 503 | ////////////////////////////////////////////////////////////////////////////// 504 | // RADIO CONFIGURATION 505 | ////////////////////////////////////////////////////////////////////////////// 506 | ////////////////////////////////////////////////////////////////////////////// 507 | 508 | 509 | ////////////////////////////////////////////////////////////////////////////// 510 | // FLIGHT_MODE 511 | // 512 | 513 | #ifndef FLIGHT_MODE_1 514 | # define FLIGHT_MODE_1 Mode::Number::STABILIZE 515 | #endif 516 | #ifndef FLIGHT_MODE_2 517 | # define FLIGHT_MODE_2 Mode::Number::STABILIZE 518 | #endif 519 | #ifndef FLIGHT_MODE_3 520 | # define FLIGHT_MODE_3 Mode::Number::STABILIZE 521 | #endif 522 | #ifndef FLIGHT_MODE_4 523 | # define FLIGHT_MODE_4 Mode::Number::STABILIZE 524 | #endif 525 | #ifndef FLIGHT_MODE_5 526 | # define FLIGHT_MODE_5 Mode::Number::STABILIZE 527 | #endif 528 | #ifndef FLIGHT_MODE_6 529 | # define FLIGHT_MODE_6 Mode::Number::STABILIZE 530 | #endif 531 | 532 | 533 | ////////////////////////////////////////////////////////////////////////////// 534 | // Throttle Failsafe 535 | // 536 | #ifndef FS_THR_VALUE_DEFAULT 537 | # define FS_THR_VALUE_DEFAULT 975 538 | #endif 539 | 540 | ////////////////////////////////////////////////////////////////////////////// 541 | // Takeoff 542 | // 543 | #ifndef PILOT_TKOFF_ALT_DEFAULT 544 | # define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff 545 | #endif 546 | 547 | 548 | ////////////////////////////////////////////////////////////////////////////// 549 | // Landing 550 | // 551 | #ifndef LAND_SPEED 552 | # define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s 553 | #endif 554 | #ifndef LAND_REPOSITION_DEFAULT 555 | # define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing 556 | #endif 557 | #ifndef LAND_WITH_DELAY_MS 558 | # define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event 559 | #endif 560 | #ifndef LAND_CANCEL_TRIGGER_THR 561 | # define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 562 | #endif 563 | #ifndef LAND_RANGEFINDER_MIN_ALT_CM 564 | #define LAND_RANGEFINDER_MIN_ALT_CM 200 565 | #endif 566 | 567 | ////////////////////////////////////////////////////////////////////////////// 568 | // Landing Detector 569 | // 570 | #ifndef LAND_DETECTOR_TRIGGER_SEC 571 | # define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing 572 | #endif 573 | #ifndef LAND_AIRMODE_DETECTOR_TRIGGER_SEC 574 | # define LAND_AIRMODE_DETECTOR_TRIGGER_SEC 3.0f // number of seconds to detect a landing in air mode 575 | #endif 576 | #ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC 577 | # define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over) 578 | #endif 579 | #ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF 580 | # define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter 581 | #endif 582 | #ifndef LAND_DETECTOR_ACCEL_MAX 583 | # define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s 584 | #endif 585 | 586 | ////////////////////////////////////////////////////////////////////////////// 587 | // Flight mode definitions 588 | // 589 | 590 | // Acro Mode 591 | #ifndef ACRO_LEVEL_MAX_ANGLE 592 | # define ACRO_LEVEL_MAX_ANGLE 3000 // maximum lean angle in trainer mode measured in centidegrees 593 | #endif 594 | 595 | #ifndef ACRO_LEVEL_MAX_OVERSHOOT 596 | # define ACRO_LEVEL_MAX_OVERSHOOT 1000 // maximum overshoot angle in trainer mode when full roll or pitch stick is held in centidegrees 597 | #endif 598 | 599 | #ifndef ACRO_BALANCE_ROLL 600 | #define ACRO_BALANCE_ROLL 1.0f 601 | #endif 602 | 603 | #ifndef ACRO_BALANCE_PITCH 604 | #define ACRO_BALANCE_PITCH 1.0f 605 | #endif 606 | 607 | #ifndef ACRO_RP_EXPO_DEFAULT 608 | #define ACRO_RP_EXPO_DEFAULT 0.3f // ACRO roll and pitch expo parameter default 609 | #endif 610 | 611 | #ifndef ACRO_Y_EXPO_DEFAULT 612 | #define ACRO_Y_EXPO_DEFAULT 0.0f // ACRO yaw expo parameter default 613 | #endif 614 | 615 | #ifndef ACRO_THR_MID_DEFAULT 616 | #define ACRO_THR_MID_DEFAULT 0.0f 617 | #endif 618 | 619 | #ifndef ACRO_RP_RATE_DEFAULT 620 | #define ACRO_RP_RATE_DEFAULT 360 // ACRO roll and pitch rotation rate parameter default in deg/s 621 | #endif 622 | 623 | #ifndef ACRO_Y_RATE_DEFAULT 624 | #define ACRO_Y_RATE_DEFAULT 202.5 // ACRO yaw rotation rate parameter default in deg/s 625 | #endif 626 | 627 | // RTL Mode 628 | #ifndef RTL_ALT_FINAL 629 | # define RTL_ALT_FINAL 0 // the altitude, in cm, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. 630 | #endif 631 | 632 | #ifndef RTL_ALT 633 | # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude 634 | #endif 635 | 636 | #ifndef RTL_ALT_MIN 637 | # define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) 638 | #endif 639 | 640 | #ifndef RTL_CLIMB_MIN_DEFAULT 641 | # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL 642 | #endif 643 | 644 | #ifndef RTL_ABS_MIN_CLIMB 645 | # define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb 646 | #endif 647 | 648 | #ifndef RTL_CONE_SLOPE_DEFAULT 649 | # define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone 650 | #endif 651 | 652 | #ifndef RTL_MIN_CONE_SLOPE 653 | # define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone 654 | #endif 655 | 656 | #ifndef RTL_LOITER_TIME 657 | # define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent 658 | #endif 659 | 660 | // AUTO Mode 661 | #ifndef WP_YAW_BEHAVIOR_DEFAULT 662 | # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 663 | #endif 664 | 665 | #ifndef YAW_LOOK_AHEAD_MIN_SPEED 666 | # define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before copter is aimed at ground course 667 | #endif 668 | 669 | // Super Simple mode 670 | #ifndef SUPER_SIMPLE_RADIUS 671 | # define SUPER_SIMPLE_RADIUS 1000 672 | #endif 673 | 674 | ////////////////////////////////////////////////////////////////////////////// 675 | // Stabilize Rate Control 676 | // 677 | #ifndef ROLL_PITCH_YAW_INPUT_MAX 678 | # define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range 679 | #endif 680 | #ifndef DEFAULT_ANGLE_MAX 681 | # define DEFAULT_ANGLE_MAX 3000 // ANGLE_MAX parameters default value 682 | #endif 683 | 684 | ////////////////////////////////////////////////////////////////////////////// 685 | // Stop mode defaults 686 | // 687 | #ifndef BRAKE_MODE_SPEED_Z 688 | # define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode 689 | #endif 690 | #ifndef BRAKE_MODE_DECEL_RATE 691 | # define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode 692 | #endif 693 | 694 | ////////////////////////////////////////////////////////////////////////////// 695 | // PosHold parameter defaults 696 | // 697 | #ifndef POSHOLD_BRAKE_RATE_DEFAULT 698 | # define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec 699 | #endif 700 | #ifndef POSHOLD_BRAKE_ANGLE_DEFAULT 701 | # define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees 702 | #endif 703 | 704 | ////////////////////////////////////////////////////////////////////////////// 705 | // Pilot control defaults 706 | // 707 | 708 | #ifndef THR_DZ_DEFAULT 709 | # define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter 710 | #endif 711 | 712 | // default maximum vertical velocity and acceleration the pilot may request 713 | #ifndef PILOT_VELZ_MAX 714 | # define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s 715 | #endif 716 | #ifndef PILOT_ACCEL_Z_DEFAULT 717 | # define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control 718 | #endif 719 | 720 | #ifndef PILOT_Y_RATE_DEFAULT 721 | # define PILOT_Y_RATE_DEFAULT 202.5 // yaw rotation rate parameter default in deg/s for all mode except ACRO 722 | #endif 723 | #ifndef PILOT_Y_EXPO_DEFAULT 724 | # define PILOT_Y_EXPO_DEFAULT 0.0 // yaw expo parameter default for all mode except ACRO 725 | #endif 726 | 727 | #ifndef AUTO_DISARMING_DELAY 728 | # define AUTO_DISARMING_DELAY 10 729 | #endif 730 | 731 | ////////////////////////////////////////////////////////////////////////////// 732 | // Throw mode configuration 733 | // 734 | #ifndef THROW_HIGH_SPEED 735 | # define THROW_HIGH_SPEED 500.0f // vehicle much reach this total 3D speed in cm/s (or be free falling) 736 | #endif 737 | #ifndef THROW_VERTICAL_SPEED 738 | # define THROW_VERTICAL_SPEED 50.0f // motors start when vehicle reaches this total 3D speed in cm/s 739 | #endif 740 | 741 | ////////////////////////////////////////////////////////////////////////////// 742 | // Logging control 743 | // 744 | #ifndef LOGGING_ENABLED 745 | # define LOGGING_ENABLED ENABLED 746 | #endif 747 | 748 | // Default logging bitmask 749 | #ifndef DEFAULT_LOG_BITMASK 750 | # define DEFAULT_LOG_BITMASK \ 751 | MASK_LOG_ATTITUDE_MED | \ 752 | MASK_LOG_GPS | \ 753 | MASK_LOG_PM | \ 754 | MASK_LOG_CTUN | \ 755 | MASK_LOG_NTUN | \ 756 | MASK_LOG_RCIN | \ 757 | MASK_LOG_IMU | \ 758 | MASK_LOG_CMD | \ 759 | MASK_LOG_CURRENT | \ 760 | MASK_LOG_RCOUT | \ 761 | MASK_LOG_OPTFLOW | \ 762 | MASK_LOG_COMPASS | \ 763 | MASK_LOG_CAMERA | \ 764 | MASK_LOG_MOTBATT 765 | #endif 766 | 767 | ////////////////////////////////////////////////////////////////////////////// 768 | // Fence, Rally and Terrain and AC_Avoidance defaults 769 | // 770 | 771 | #ifndef AC_AVOID_ENABLED 772 | #define AC_AVOID_ENABLED ENABLED 773 | #endif 774 | 775 | #ifndef AC_OAPATHPLANNER_ENABLED 776 | #define AC_OAPATHPLANNER_ENABLED !HAL_MINIMIZE_FEATURES 777 | #endif 778 | 779 | #if MODE_FOLLOW_ENABLED && !AC_AVOID_ENABLED 780 | #error Follow Mode relies on AC_AVOID which is disabled 781 | #endif 782 | 783 | #if MODE_AUTO_ENABLED && !MODE_GUIDED_ENABLED 784 | #error ModeAuto requires ModeGuided which is disabled 785 | #endif 786 | 787 | #if MODE_AUTO_ENABLED && !MODE_CIRCLE_ENABLED 788 | #error ModeAuto requires ModeCircle which is disabled 789 | #endif 790 | 791 | #if MODE_AUTO_ENABLED && !MODE_RTL_ENABLED 792 | #error ModeAuto requires ModeRTL which is disabled 793 | #endif 794 | 795 | #if FRAME_CONFIG == HELI_FRAME && !MODE_ACRO_ENABLED 796 | #error Helicopter frame requires acro mode support which is disabled 797 | #endif 798 | 799 | #if MODE_SMARTRTL_ENABLED && !MODE_RTL_ENABLED 800 | #error SmartRTL requires ModeRTL which is disabled 801 | #endif 802 | 803 | #if HAL_ADSB_ENABLED && !MODE_GUIDED_ENABLED 804 | #error ADSB requires ModeGuided which is disabled 805 | #endif 806 | 807 | #if MODE_FOLLOW_ENABLED && !MODE_GUIDED_ENABLED 808 | #error Follow requires ModeGuided which is disabled 809 | #endif 810 | 811 | #if MODE_GUIDED_NOGPS_ENABLED && !MODE_GUIDED_ENABLED 812 | #error ModeGuided-NoGPS requires ModeGuided which is disabled 813 | #endif 814 | 815 | ////////////////////////////////////////////////////////////////////////////// 816 | // Developer Items 817 | // 818 | 819 | #ifndef ADVANCED_FAILSAFE 820 | # define ADVANCED_FAILSAFE DISABLED 821 | #endif 822 | 823 | #ifndef CH_MODE_DEFAULT 824 | # define CH_MODE_DEFAULT 5 825 | #endif 826 | 827 | #ifndef TOY_MODE_ENABLED 828 | #define TOY_MODE_ENABLED DISABLED 829 | #endif 830 | 831 | #if TOY_MODE_ENABLED && FRAME_CONFIG == HELI_FRAME 832 | #error Toy mode is not available on Helicopters 833 | #endif 834 | 835 | #ifndef STATS_ENABLED 836 | # define STATS_ENABLED ENABLED 837 | #endif 838 | 839 | #ifndef OSD_ENABLED 840 | #define OSD_ENABLED DISABLED 841 | #endif 842 | 843 | #ifndef HAL_FRAME_TYPE_DEFAULT 844 | #define HAL_FRAME_TYPE_DEFAULT AP_Motors::MOTOR_FRAME_TYPE_X 845 | #endif 846 | 847 | #ifndef AC_CUSTOMCONTROL_MULTI_ENABLED 848 | #define AC_CUSTOMCONTROL_MULTI_ENABLED FRAME_CONFIG == MULTICOPTER_FRAME && AP_CUSTOMCONTROL_ENABLED 849 | #endif 850 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/ACRL_trajectories.cpp: -------------------------------------------------------------------------------- 1 | #include "ACRL_trajectories.h" 2 | 3 | // ACRL_trajectories.cpp 4 | // This file contains the trajectories for running ACRL's adaptive flight mode. 5 | // Copyright 2021 Sheng Cheng, all rights reserved. 6 | 7 | // Dr. Sheng Cheng, Nov. 2021 8 | // Email: chengs@illinois.edu 9 | // Advanced Controls Research Laboratory 10 | // Department of Mechanical Science and Engineering 11 | // University of Illinois Urbana-Champaign 12 | // Urbana, IL 61821, USA 13 | 14 | void ACRL_trajectory_takeoff(float timeInThisRun, 15 | Vector3f *targetPos, 16 | Vector3f *targetVel, 17 | Vector3f *targetAcc, 18 | Vector3f *targetJerk, 19 | Vector3f *targetSnap, 20 | Vector2f *targetYaw, 21 | Vector2f *targetYaw_dot, 22 | Vector2f *targetYaw_ddot) 23 | { 24 | // The function ACRL_trajectory_takeoff function evaluates the takeoff trajectory. 25 | // The order of the polynomial coefficients follow the matlab definition. 26 | float polyCoef[8] = {-0.1563, 1.0938, -2.6250, 2.1875, 0, 0, 0, 0}; 27 | if (timeInThisRun < 2) // only takeoff during the first 2 seconds when entering mode ADAPTIVE 28 | { 29 | *targetPos = (Vector3f){0, 0, -polyEval(polyCoef, timeInThisRun, 8)}; 30 | 31 | *targetVel = (Vector3f){0, 0, -polyDiffEval(polyCoef, timeInThisRun, 8)}; 32 | 33 | *targetAcc = (Vector3f){0, 0, -polyDiff2Eval(polyCoef, timeInThisRun, 8)}; 34 | 35 | *targetJerk = (Vector3f){0, 0, -polyDiff3Eval(polyCoef, timeInThisRun, 8)}; 36 | 37 | *targetSnap = (Vector3f){0, 0, -polyDiff4Eval(polyCoef, timeInThisRun, 8)}; 38 | 39 | *targetYaw = (Vector2f){1, 0}; 40 | *targetYaw_dot = (Vector2f){0, 0}; 41 | *targetYaw_ddot = (Vector2f){0, 0}; 42 | } 43 | else 44 | { 45 | // print the error information that the quadrotor cannot take off 46 | gcs().send_text(MAV_SEVERITY_INFO, "Quadrotor cannot takeoff: time is %f > 2 s.", timeInThisRun); 47 | } 48 | } 49 | 50 | void ACRL_trajectory_transition_to_start(float timeInThisRun, 51 | float radiusX, 52 | float timeOffset, 53 | Vector3f *targetPos, 54 | Vector3f *targetVel, 55 | Vector3f *targetAcc, 56 | Vector3f *targetJerk, 57 | Vector3f *targetSnap, 58 | Vector2f *targetYaw, 59 | Vector2f *targetYaw_dot, 60 | Vector2f *targetYaw_ddot) 61 | { 62 | // The function ACRL_trajectory_transition_to_start function evaluates transition trajectory from (0,0,-1) to (0,-radiusX, -1), where radiusX is the circle radius. 63 | // The order of the polynomial coefficients follow the matlab definition. 64 | float polyCoef[8] = {-0.1563, 1.0938, -2.6250, 2.1875, 0, 0, 0, 0}; 65 | if (timeInThisRun >= timeOffset && timeInThisRun <= timeOffset + 2) // only do the transition in 2-4 seconds when entering mode ADAPTIVE 66 | { 67 | *targetPos = (Vector3f){0, -radiusX * polyEval(polyCoef, timeInThisRun - timeOffset, 8), -1}; 68 | 69 | *targetVel = (Vector3f){0, -radiusX * polyDiffEval(polyCoef, timeInThisRun - timeOffset, 8), 0}; 70 | 71 | *targetAcc = (Vector3f){0, -radiusX * polyDiff2Eval(polyCoef, timeInThisRun - timeOffset, 8), 0}; 72 | 73 | *targetJerk = (Vector3f){0, -radiusX * polyDiff3Eval(polyCoef, timeInThisRun - timeOffset, 8), 0}; 74 | 75 | *targetSnap = (Vector3f){0, -radiusX * polyDiff4Eval(polyCoef, timeInThisRun - timeOffset, 8), 0}; 76 | 77 | *targetYaw = (Vector2f){1, 0}; 78 | *targetYaw_dot = (Vector2f){0, 0}; 79 | *targetYaw_ddot = (Vector2f){0, 0}; 80 | } 81 | else 82 | { 83 | // print the error information that the quadrotor cannot take off 84 | gcs().send_text(MAV_SEVERITY_INFO, "Quadrotor can't transition to circle start position: time is %f, not in [2,4] s.", timeInThisRun); 85 | } 86 | } 87 | 88 | // ACRL_trajectory_transition_to_start(timeInThisRun, radiusX, &targetPos, &targetVel, &targetAcc, &targetJerk, &targetSnap, &targetYaw, &targetYaw_dot, &targetYaw_ddot); 89 | 90 | void ACRL_trajectory_circle_variable_yaw(float timeInThisRun, 91 | float radius, 92 | float initialTimeOffset, 93 | float targetSpeed, 94 | Vector3f *targetPos, 95 | Vector3f *targetVel, 96 | Vector3f *targetAcc, 97 | Vector3f *targetJerk, 98 | Vector3f *targetSnap, 99 | Vector2f *targetYaw, 100 | Vector2f *targetYaw_dot, 101 | Vector2f *targetYaw_ddot) 102 | { 103 | // The function ACRL_trajectory_variable_yaw function evaluates the circular trajectory that always aligns the yaw angle with the tangent speed direction. 104 | static float currentSpeed = 0; // current speed of the rotation 105 | static float timeOffset = 0; // offset in time for the circular trajectory at different speeds 106 | static float currentLoopTime = 0; // time to complete one circle under current speed 107 | float netTime = 0; // time for the circular trajectory at different speeds 108 | static u_int8_t accComplete = 0; // indicator for whether acceleration is complete 109 | 110 | if ((currentSpeed <= 0.1) && (targetSpeed >= 0.5)) // initialize and only accelerate for targetSpeed >= 0.51 m/s 111 | { 112 | currentSpeed = 0.5 / radius; // initialize as 0.5 m/s (currentSpeed is angular rate, the linear speed is currentSpeed * radius) 113 | timeOffset = initialTimeOffset; // initial time for starting the trajectory 114 | currentLoopTime = 6.28 / currentSpeed; // time to complete one circle under current speed: (2 * pi * radius) / (currentSpeed * radius) 115 | gcs().send_text(MAV_SEVERITY_INFO, "Acceleration initiated."); 116 | gcs().send_text(MAV_SEVERITY_INFO, "Initial speed at 0.5 m/s (complete in %f s)", currentLoopTime); 117 | } 118 | else if ((timeInThisRun - timeOffset >= currentLoopTime) && (!accComplete)) // if the drone has completed one circle at the current speed and acceleration is not complete 119 | { 120 | if ((currentSpeed*radius) >= 1.98) // increase speed by 0.1 m/s if the linear speed goes beyond 1.98 m/s (approx. 2 m/s) 121 | { 122 | currentSpeed += 0.1 / radius; 123 | } 124 | else // otherwise, increase speed by 0.5 m/s 125 | { 126 | currentSpeed += 0.5 / radius; 127 | } 128 | if (currentSpeed > (targetSpeed / radius)) 129 | { 130 | timeOffset += currentLoopTime; // update the loop time 131 | currentSpeed = targetSpeed / radius; // set current speed to the circspeed 132 | accComplete = 1; // flag the acceleration as complete 133 | gcs().send_text(MAV_SEVERITY_INFO, "Reached target speed %f m/s", (double)targetSpeed); 134 | } 135 | else 136 | { 137 | timeOffset += currentLoopTime; // update the loop time 138 | currentLoopTime = 6.28 / currentSpeed; // time to complete one circle under current speed 139 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentSpeed * radius, currentLoopTime); 140 | } 141 | } 142 | netTime = timeInThisRun - timeOffset; // time reference for this speed 143 | 144 | #if (!REAL_OR_SITL) // SITL 145 | *targetPos = (Vector3f){radius * sinf(currentSpeed * netTime), radius * (1 - cosf(currentSpeed * netTime)), -1}; 146 | #elif (REAL_OR_SITL) // Real 147 | *targetPos = (Vector3f){radius * sinf(currentSpeed * netTime), radius * (-cosf(currentSpeed * netTime)), -1}; 148 | #endif 149 | 150 | *targetVel = (Vector3f){radius * currentSpeed * cosf(currentSpeed * netTime), radius * currentSpeed * sinf(currentSpeed * netTime), 0}; 151 | 152 | *targetAcc = (Vector3f){-radius * powf(currentSpeed, 2) * sinf(currentSpeed * netTime), radius * powf(currentSpeed, 2) * cosf(currentSpeed * netTime), 0}; 153 | 154 | *targetJerk = (Vector3f){-radius * powf(currentSpeed, 3) * cosf(currentSpeed * netTime), -radius * powf(currentSpeed, 3) * sinf(currentSpeed * netTime), 0}; 155 | 156 | *targetSnap = (Vector3f){radius * powf(currentSpeed, 4) * sinf(currentSpeed * netTime), -radius * powf(currentSpeed, 4) * cosf(currentSpeed * netTime), 0}; 157 | 158 | *targetYaw = (Vector2f){cosF(currentSpeed * netTime), sinF(currentSpeed * netTime)}; 159 | *targetYaw_dot = (Vector2f){-currentSpeed * sinF(currentSpeed * netTime), currentSpeed * cosF(currentSpeed * netTime)}; 160 | *targetYaw_ddot = (Vector2f){-powf(currentSpeed, 2) * cosF(currentSpeed * netTime), -powf(currentSpeed, 2) * sinF(currentSpeed * netTime)}; 161 | } 162 | 163 | void ACRL_trajectory_circle_fixed_yaw(float timeInThisRun, 164 | float radius, 165 | float initialTimeOffset, 166 | float targetSpeed, 167 | Vector3f *targetPos, 168 | Vector3f *targetVel, 169 | Vector3f *targetAcc, 170 | Vector3f *targetJerk, 171 | Vector3f *targetSnap, 172 | Vector2f *targetYaw, 173 | Vector2f *targetYaw_dot, 174 | Vector2f *targetYaw_ddot) 175 | { 176 | // The function ACRL_trajectory_fixed_yaw function evaluates the circular trajectory that has a fixed yaw angle (=0). 177 | static float currentSpeed = 0; // current speed of the rotation 178 | static float timeOffset = 0; // offset in time for the circular trajectory at different speeds 179 | static float currentLoopTime = 0; // time to complete one circle under current speed 180 | float netTime = 0; // time for the circular trajectory at different speeds 181 | static u_int8_t accComplete = 0; // indicator for whether acceleration is complete 182 | 183 | if ((currentSpeed <= 0.1) && (targetSpeed >= 0.5)) // initialize and only accelerate for targetSpeed >= 0.51 m/s 184 | { 185 | currentSpeed = 0.5 / radius; // initialize as 0.5 m/s (currentSpeed is angular rate, the linear speed is currentSpeed * radius) 186 | timeOffset = initialTimeOffset; // initial time for starting the trajectory 187 | currentLoopTime = 6.28 / currentSpeed; // time to complete one circle under current speed: 2 * pi * radius / currentSpeed 188 | gcs().send_text(MAV_SEVERITY_INFO, "Acceleration initiated."); 189 | gcs().send_text(MAV_SEVERITY_INFO, "Initial speed at 0.5 m/s (complete in %f s)", currentLoopTime); 190 | } 191 | else if ((timeInThisRun - timeOffset >= currentLoopTime) && (!accComplete)) // if the drone has completed one circle at the current speed and acceleration is not complete 192 | { 193 | if ((currentSpeed*radius) >= 1.98) // increase speed by 0.1 m/s if the linear speed goes beyond 1.98 m/s (approx. 2 m/s) 194 | { 195 | currentSpeed += 0.1 / radius; 196 | } 197 | else // otherwise, increase speed by 0.5 m/s 198 | { 199 | currentSpeed += 0.5 / radius; 200 | } 201 | if (currentSpeed > (targetSpeed / radius)) 202 | { 203 | timeOffset += currentLoopTime; // update the loop time 204 | currentSpeed = targetSpeed / radius; // set current speed to the circspeed 205 | accComplete = 1; // flag the acceleration as complete 206 | gcs().send_text(MAV_SEVERITY_INFO, "Reached target speed %f m/s", (double)targetSpeed); 207 | } 208 | else 209 | { 210 | timeOffset += currentLoopTime; // update the loop time 211 | currentLoopTime = 6.28 / currentSpeed; // time to complete one circle under current speed 212 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentSpeed * radius, currentLoopTime); 213 | } 214 | } 215 | netTime = timeInThisRun - timeOffset; // time reference for this speed 216 | 217 | #if (!REAL_OR_SITL) // SITL 218 | *targetPos = (Vector3f){radius * sinf(currentSpeed * netTime), radius * (1 - cosf(currentSpeed * netTime)), -1}; 219 | #elif (REAL_OR_SITL) // Real 220 | *targetPos = (Vector3f){radius * sinf(currentSpeed * netTime), radius * (-cosf(currentSpeed * netTime)), -1}; 221 | #endif 222 | 223 | *targetVel = (Vector3f){radius * currentSpeed * cosf(currentSpeed * netTime), radius * currentSpeed * sinf(currentSpeed * netTime), 0}; 224 | 225 | *targetAcc = (Vector3f){-radius * powf(currentSpeed, 2) * sinf(currentSpeed * netTime), radius * powf(currentSpeed, 2) * cosf(currentSpeed * netTime), 0}; 226 | 227 | *targetJerk = (Vector3f){-radius * powf(currentSpeed, 3) * cosf(currentSpeed * netTime), -radius * powf(currentSpeed, 3) * sinf(currentSpeed * netTime), 0}; 228 | 229 | *targetSnap = (Vector3f){radius * powf(currentSpeed, 4) * sinf(currentSpeed * netTime), -radius * powf(currentSpeed, 4) * cosf(currentSpeed * netTime), 0}; 230 | 231 | *targetYaw = (Vector2f){1, 0}; 232 | *targetYaw_dot = (Vector2f){0, 0}; 233 | *targetYaw_ddot = (Vector2f){0, 0}; 234 | } 235 | 236 | void ACRL_trajectory_figure8_fixed_yaw(float timeInThisRun, 237 | float radiusX, 238 | float radiusY, 239 | float targetSpeed, 240 | Vector3f *targetPos, 241 | Vector3f *targetVel, 242 | Vector3f *targetAcc, 243 | Vector3f *targetJerk, 244 | Vector3f *targetSnap, 245 | Vector2f *targetYaw, 246 | Vector2f *targetYaw_dot, 247 | Vector2f *targetYaw_ddot) 248 | { 249 | // The function ACRL_trajectory_figure8_fixed_yaw function evaluates the figure8 trajectory that has a fixed yaw angle (=0). 250 | static float currentRate = 0; // current max speed at origin 251 | static float currentEqvRate = 0; // current equivalent speed to ease computation 252 | static float timeOffset = 0; // offset in time for the circular trajectory at different speeds 253 | static float currentLoopTime = 0; // time to complete one circle under current speed 254 | float netTime = 0; // time for the circular trajectory at different speeds 255 | static u_int8_t accComplete = 0; // indicator for whether acceleration is complete 256 | static uint8_t positiveLoop = 1; // This is the indicator of positive loop, where the drone flies in the positive region of x 257 | // The value of positiveLoop is 0 if the drone flies in the negative loop 258 | 259 | const float scaleFactor = sqrtf(radiusX * radiusX + 4 * radiusY * radiusY); // scale the coefficients for the cos/sin functions to the speed so the final speed when passing through the origin equals targetSpeed 260 | 261 | if ((currentRate <= 0.1) && (targetSpeed >= 0.5)) // initialize and only accelerate for targetSpeed >= 0.5 m/s 262 | { 263 | currentRate = 0.5; // initialize as 0.5 m/s 264 | currentEqvRate = currentRate / scaleFactor; 265 | timeOffset = 2; // initial time after takeoff 266 | currentLoopTime = 3.14 / currentEqvRate; // time to complete one circle under current speed: 2 * pi * radius / currentRate 267 | gcs().send_text(MAV_SEVERITY_INFO, "Acceleration initiated."); 268 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentRate, currentLoopTime); 269 | } 270 | else if ((timeInThisRun - timeOffset >= currentLoopTime) && (!accComplete)) // if the drone has completed one circle at the current speed and acceleration is not complete 271 | { 272 | if (abs(currentRate - targetSpeed) <= 0.01) // acceleration complete 273 | { 274 | currentRate = targetSpeed; // set current speed to the circspeed 275 | currentEqvRate = currentRate / scaleFactor; 276 | accComplete = 1; // flag the acceleration as complete 277 | gcs().send_text(MAV_SEVERITY_INFO, "Reached target speed %f m/s", (double)targetSpeed); 278 | } 279 | else // accelerating 280 | { 281 | currentRate += 0.5; // increase speed by 0.5 282 | if (currentRate > targetSpeed) // fix the currentRate if it goes beyond target speed 283 | { 284 | currentRate = targetSpeed; 285 | } 286 | currentEqvRate = currentRate / scaleFactor; 287 | timeOffset += currentLoopTime; // update the loop time 288 | currentLoopTime = 3.14 / currentEqvRate; // time to complete one circle under current speed 289 | if (positiveLoop) // flip the direction upon acceleration 290 | { 291 | positiveLoop = 0; 292 | } 293 | else 294 | { 295 | positiveLoop = 1; 296 | } 297 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentRate, currentLoopTime); 298 | } 299 | } 300 | netTime = timeInThisRun - timeOffset; // time reference for this speed 301 | 302 | float sf = sinf(currentEqvRate * netTime); 303 | float s2f = sinf(2 * currentEqvRate * netTime); 304 | float cf = cosf(currentEqvRate * netTime); 305 | float c2f = cosf(2 * currentEqvRate * netTime); 306 | 307 | if (positiveLoop) 308 | { 309 | *targetPos = (Vector3f){radiusX * sf, radiusY * s2f, -1}; 310 | 311 | *targetVel = (Vector3f){radiusX * currentEqvRate * cf, radiusY * 2 * currentEqvRate * c2f, 0}; 312 | 313 | *targetAcc = (Vector3f){-radiusX * currentEqvRate * currentEqvRate * sf, -radiusY * 4 * currentEqvRate * currentEqvRate * s2f, 0}; 314 | 315 | *targetJerk = (Vector3f){-radiusX * powf(currentEqvRate, 3) * cf, -radiusY * 8 * powf(currentEqvRate, 3) * c2f, 0}; 316 | 317 | *targetSnap = (Vector3f){radiusX * powf(currentEqvRate, 4) * sf, radiusY * 16 * powf(currentEqvRate, 4) * s2f, 0}; 318 | } 319 | else 320 | { 321 | *targetPos = (Vector3f){-radiusX * sf, radiusY * s2f, -1}; 322 | 323 | *targetVel = (Vector3f){-radiusX * currentEqvRate * cf, radiusY * 2 * currentEqvRate * c2f, 0}; 324 | 325 | *targetAcc = (Vector3f){radiusX * currentEqvRate * currentEqvRate * sf, -radiusY * 4 * currentEqvRate * currentEqvRate * s2f, 0}; 326 | 327 | *targetJerk = (Vector3f){radiusX * powf(currentEqvRate, 3) * cf, -radiusY * 8 * powf(currentEqvRate, 3) * c2f, 0}; 328 | 329 | *targetSnap = (Vector3f){-radiusX * powf(currentEqvRate, 4) * sf, radiusY * 16 * powf(currentEqvRate, 4) * s2f, 0}; 330 | } 331 | *targetYaw = (Vector2f){1, 0}; 332 | *targetYaw_dot = (Vector2f){0, 0}; 333 | *targetYaw_ddot = (Vector2f){0, 0}; 334 | } 335 | 336 | // trajectory 4 337 | void ACRL_trajectory_figure8_tilted(float timeInThisRun, 338 | float radiusX, 339 | float radiusY, 340 | float targetSpeed, 341 | Vector3f *targetPos, 342 | Vector3f *targetVel, 343 | Vector3f *targetAcc, 344 | Vector3f *targetJerk, 345 | Vector3f *targetSnap, 346 | Vector2f *targetYaw, 347 | Vector2f *targetYaw_dot, 348 | Vector2f *targetYaw_ddot) 349 | { 350 | // The function ACRL_trajectory_figure8_tilted function evaluates the figure8 trajectory that has a tilted altitude. 351 | static float currentRate = 0; // current max speed at origin 352 | static float currentEqvRate = 0; // current equivalent speed to ease computation 353 | static float timeOffset = 0; // offset in time for the circular trajectory at different speeds 354 | static float currentLoopTime = 0; // time to complete one circle under current speed 355 | float netTime = 0; // time for the circular trajectory at different speeds 356 | static u_int8_t accComplete = 0; // indicator for whether acceleration is complete 357 | static uint8_t positiveLoop = 1; // This is the indicator of positive loop, where the drone flies in the positive region of x 358 | // The value of positiveLoop is 0 if the drone flies in the negative loop 359 | 360 | float z_amp = 0.2; // amplitude of tilted altitude (z axis) 361 | const float scaleFactor = sqrtf(radiusX * radiusX + 4 * radiusY * radiusY + z_amp * z_amp); // scale the coefficients for the cos/sin functions to the speed so the final speed when passing through the origin equals targetSpeed 362 | 363 | if ((currentRate <= 0.1) && (targetSpeed >= 0.5)) // initialize and only accelerate for targetSpeed >= 0.5 m/s 364 | { 365 | currentRate = 0.5; // initialize as 0.5 m/s 366 | currentEqvRate = currentRate / scaleFactor; 367 | timeOffset = 2; // initial time after takeoff 368 | currentLoopTime = 3.14 / currentEqvRate; // time to complete one circle under current speed: 2 * pi * radius / currentRate 369 | gcs().send_text(MAV_SEVERITY_INFO, "Acceleration initiated."); 370 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentRate, currentLoopTime); 371 | } 372 | else if ((timeInThisRun - timeOffset >= currentLoopTime) && (!accComplete)) // if the drone has completed one circle at the current speed and acceleration is not complete 373 | { 374 | if (abs(currentRate - targetSpeed) <= 0.01) // acceleration complete 375 | { 376 | currentRate = targetSpeed; // set current speed to the circspeed 377 | currentEqvRate = currentRate / scaleFactor; 378 | accComplete = 1; // flag the acceleration as complete 379 | gcs().send_text(MAV_SEVERITY_INFO, "Reached target speed %f m/s", (double)targetSpeed); 380 | } 381 | else // accelerating 382 | { 383 | currentRate += 0.5; // increase speed by 0.5 384 | if (currentRate > targetSpeed) // fix the currentRate if it goes beyond target speed 385 | { 386 | currentRate = targetSpeed; 387 | } 388 | currentEqvRate = currentRate / scaleFactor; 389 | timeOffset += currentLoopTime; // update the loop time 390 | currentLoopTime = 3.14 / currentEqvRate; // time to complete one circle under current speed 391 | if (positiveLoop) // flip the direction upon acceleration 392 | { 393 | positiveLoop = 0; 394 | } 395 | else 396 | { 397 | positiveLoop = 1; 398 | } 399 | gcs().send_text(MAV_SEVERITY_INFO, "Starting %f m/s (complete in %f s)", currentRate, currentLoopTime); 400 | } 401 | } 402 | netTime = timeInThisRun - timeOffset; // time reference for this speed 403 | 404 | float sf = sinf(currentEqvRate * netTime); 405 | float s2f = sinf(2 * currentEqvRate * netTime); 406 | float cf = cosf(currentEqvRate * netTime); 407 | float c2f = cosf(2 * currentEqvRate * netTime); 408 | 409 | if (positiveLoop) 410 | { 411 | *targetPos = (Vector3f){radiusX * sf, radiusY * s2f, -1 - z_amp * sf}; 412 | 413 | *targetVel = (Vector3f){radiusX * currentEqvRate * cf, radiusY * 2 * currentEqvRate * c2f, -z_amp * currentEqvRate * cf}; 414 | 415 | *targetAcc = (Vector3f){-radiusX * currentEqvRate * currentEqvRate * sf, -radiusY * 4 * currentEqvRate * currentEqvRate * s2f, z_amp * currentEqvRate * currentEqvRate * sf}; 416 | 417 | *targetJerk = (Vector3f){-radiusX * powf(currentEqvRate, 3) * cf, -radiusY * 8 * powf(currentEqvRate, 3) * c2f, z_amp * powf(currentEqvRate, 3) * cf}; 418 | 419 | *targetSnap = (Vector3f){radiusX * powf(currentEqvRate, 4) * sf, radiusY * 16 * powf(currentEqvRate, 4) * s2f, -z_amp * powf(currentEqvRate, 4) * sf}; 420 | } 421 | else 422 | { 423 | *targetPos = (Vector3f){-radiusX * sf, radiusY * s2f, -1 + z_amp * sf}; 424 | 425 | *targetVel = (Vector3f){-radiusX * currentEqvRate * cf, radiusY * 2 * currentEqvRate * c2f, z_amp * currentEqvRate * cf}; 426 | 427 | *targetAcc = (Vector3f){radiusX * currentEqvRate * currentEqvRate * sf, -radiusY * 4 * currentEqvRate * currentEqvRate * s2f, -z_amp * currentEqvRate * currentEqvRate * sf}; 428 | 429 | *targetJerk = (Vector3f){radiusX * powf(currentEqvRate, 3) * cf, -radiusY * 8 * powf(currentEqvRate, 3) * c2f, -z_amp * powf(currentEqvRate, 3) * cf}; 430 | 431 | *targetSnap = (Vector3f){-radiusX * powf(currentEqvRate, 4) * sf, radiusY * 16 * powf(currentEqvRate, 4) * s2f, z_amp * powf(currentEqvRate, 4) * sf}; 432 | } 433 | 434 | *targetYaw = (Vector2f){1, 0}; 435 | *targetYaw_dot = (Vector2f){0, 0}; 436 | *targetYaw_ddot = (Vector2f){0, 0}; 437 | } 438 | 439 | // landing trajectory 440 | uint8_t ACRL_trajectory_land(float currentTime, 441 | Vector3f currentPosition, 442 | Vector3f currentVelocity, 443 | float currentYaw, 444 | float decRate, 445 | Vector3f *targetPos, 446 | Vector3f *targetVel, 447 | Vector3f *targetAcc, 448 | Vector3f *targetJerk, 449 | Vector3f *targetSnap, 450 | Vector2f *targetYaw, 451 | Vector2f *targetYaw_dot, 452 | Vector2f *targetYaw_ddot) 453 | { 454 | // The function ACRL_trajectory_land function executes the landing procedure 455 | // The landing procedure first hault the quadrotor into hover for 2 seconds 456 | // Then the quadrotor will follow a descending trajectory to complete the descending in 3 seconds 457 | 458 | static Vector3f enterPosition; // position when landing/hover triggered 459 | static Vector3f enterVelocity; // velocity when landing/hover triggered 460 | static float enterYaw; // yaw angle when landing/hover triggered 461 | static float enterLinVelInv; // inverse of linear velocity when landing triggered 462 | static u_int8_t decComplete = 0; // indicator for whether deceleration is complete 463 | static float switchSpeed = 0.8; // switch to hover when less than this speed 464 | static float decTime = 0; // time for deceleration. 3 times of deceleration at most 465 | const float hoverTime = 3; // time for hover before landing in seconds 466 | const float landTime = 3; // time for landing in seconds 467 | float linearVelocity = 0; // calculate the linear velocity based on currentVelocity 468 | linearVelocity = sqrtf(currentVelocity[0] * currentVelocity[0] + currentVelocity[1] * currentVelocity[1] + currentVelocity[2] * currentVelocity[2]); 469 | 470 | if (currentTime < 0.0025) 471 | { 472 | enterPosition = currentPosition; 473 | enterVelocity = currentVelocity; 474 | enterYaw = currentYaw; 475 | enterLinVelInv = 1 / linearVelocity; 476 | decTime = (linearVelocity - switchSpeed) / decRate; // switch to hover at 1m/s 477 | 478 | gcs().send_text(MAV_SEVERITY_INFO, "current position = (%.3f, %.3f, %.3f), current velocity = %.3f", enterPosition[0], enterPosition[1], enterPosition[2], linearVelocity); 479 | } 480 | if ((linearVelocity > switchSpeed) && (!decComplete)) // decelerating 481 | { 482 | if (currentTime < 0.0025) 483 | { 484 | gcs().send_text(MAV_SEVERITY_INFO, "deceleration rate = %.3f, decelerate in %.3f seconds", decRate, decTime); 485 | } 486 | float decTimeInv = 1 / decTime; 487 | *targetVel = (Vector3f){enterVelocity[0] * (1 - currentTime * decTimeInv) + switchSpeed * enterVelocity[0] * enterLinVelInv * currentTime * decTimeInv, enterVelocity[1] * (1 - currentTime * decTimeInv) + switchSpeed * enterVelocity[1] * enterLinVelInv * currentTime * decTimeInv, 0}; 488 | *targetPos = (Vector3f){enterPosition[0] + (enterVelocity[0] + (targetVel->x)) * currentTime / 2, enterPosition[1] + (enterVelocity[1] + (targetVel->y)) * currentTime / 2, enterPosition[2]}; 489 | 490 | *targetAcc = (Vector3f){-powf(-1, signbit(enterVelocity[0])) * decTimeInv * (1 - switchSpeed * enterLinVelInv), -powf(-1, signbit(enterVelocity[1])) * decTimeInv * (1 - switchSpeed * enterLinVelInv), 0}; 491 | *targetJerk = (Vector3f){0,0,0}; 492 | *targetSnap = (Vector3f){0,0,0}; 493 | 494 | *targetYaw = (Vector2f){cosf(enterYaw), sinf(enterYaw)}; 495 | *targetYaw_dot = (Vector2f){0, 0}; 496 | *targetYaw_ddot = (Vector2f){0, 0}; 497 | 498 | return (uint8_t) 0; // landing is not completed, return 0 499 | } 500 | else // hovering and landing 501 | { 502 | if (!decComplete) 503 | { 504 | gcs().send_text(MAV_SEVERITY_INFO, "Landing mode phase 0: Deceleration complete"); 505 | decComplete = 1; 506 | enterPosition = currentPosition; 507 | enterVelocity = currentVelocity; 508 | enterYaw = currentYaw; 509 | decTime = currentTime; 510 | } 511 | if (currentTime <= decTime + hoverTime) // hover the quadrotor at the current position 512 | { 513 | if (currentTime <= decTime + 0.005) 514 | { 515 | gcs().send_text(MAV_SEVERITY_INFO, "Landing mode phase 1: Hovering for %f seconds", hoverTime); 516 | } 517 | *targetPos = enterPosition; // currentPosition works for SITL, but cannot provide stable behavior in real cases 518 | *targetVel = (Vector3f){0,0,0}; 519 | *targetAcc = (Vector3f){0,0,0}; 520 | *targetJerk = (Vector3f){0,0,0}; 521 | *targetSnap = (Vector3f){0,0,0}; 522 | 523 | *targetYaw = (Vector2f){cosf(enterYaw), sinf(enterYaw)}; 524 | *targetYaw_dot = (Vector2f){0, 0}; 525 | *targetYaw_ddot = (Vector2f){0, 0}; 526 | 527 | return (uint8_t) 0; // landing is not completed, return 0 528 | } 529 | else if ((currentTime > decTime + hoverTime) && (currentTime <= decTime + hoverTime + landTime)) // execute the land trajectory 530 | { 531 | printf("currentTime is %f\n.",currentTime); 532 | if (currentTime <= decTime + hoverTime + 0.005) 533 | { 534 | gcs().send_text(MAV_SEVERITY_INFO, "Landing mode phase 2: Descending for %f seconds", landTime); 535 | } 536 | 537 | float polyCoef[8] = {-0.00122109375, 0.017090625, -0.08203125, 0.13671875,0,0,0,0}; 538 | *targetPos = (Vector3f) {enterPosition[0],enterPosition[1],enterPosition[2] - enterPosition[2] * polyEval(polyCoef, currentTime - decTime - hoverTime, 8)}; 539 | *targetVel = (Vector3f){0,0,-enterPosition[2] * polyDiffEval(polyCoef, currentTime - decTime - hoverTime, 8)}; 540 | *targetAcc = (Vector3f){0,0,-enterPosition[2] * polyDiff2Eval(polyCoef, currentTime - decTime - hoverTime, 8)}; 541 | *targetJerk = (Vector3f){0,0,-enterPosition[2] * polyDiff3Eval(polyCoef, currentTime - decTime - hoverTime, 8)}; 542 | *targetSnap = (Vector3f){0,0,-enterPosition[2] * polyDiff4Eval(polyCoef, currentTime - decTime - hoverTime, 8)}; 543 | 544 | *targetYaw = (Vector2f){cosf(enterYaw), sinf(enterYaw)}; 545 | *targetYaw_dot = (Vector2f){0, 0}; 546 | *targetYaw_ddot = (Vector2f){0, 0}; 547 | 548 | return (uint8_t) 0; // landing is not completed, return 0 549 | } 550 | else // landing is completed 551 | { 552 | if (currentTime < decTime + hoverTime + landTime + 0.005) 553 | { 554 | gcs().send_text(MAV_SEVERITY_INFO, "Landing completed. Please disarm by hand."); 555 | } 556 | 557 | // load dummy trajectory (to be used by the controllers) 558 | *targetPos = (Vector3f) {enterPosition[0],enterPosition[1],0}; 559 | *targetVel = (Vector3f){0,0,0}; 560 | *targetAcc = (Vector3f){0,0,0}; 561 | *targetJerk = (Vector3f){0,0,0}; 562 | *targetSnap = (Vector3f){0,0,0}; 563 | 564 | *targetYaw = (Vector2f){cosf(enterYaw), sinf(enterYaw)}; 565 | *targetYaw_dot = (Vector2f){0, 0}; 566 | *targetYaw_ddot = (Vector2f){0, 0}; 567 | 568 | return (uint8_t) 1; // landing is completed, return 1 569 | } 570 | } 571 | } 572 | 573 | float polyEval(float polyCoef[], float x, int N) 574 | { 575 | // Evaluate the polynomial given by polyCoef at variable x. 576 | // The order of polynomial is N. 577 | float result = 0; 578 | for (int i = 0; i < N; i++) 579 | { 580 | result += polyCoef[i] * powf(x, N - 1 - i); 581 | } 582 | return result; 583 | } 584 | 585 | float polyDiffEval(float polyCoef[], float x, int N) 586 | { 587 | // Evaluate the polynomial's derivative given by polyCoef at variable x. 588 | // The order of polynomial is N. 589 | float result = 0; 590 | for (int i = 0; i < N - 1; i++) 591 | { 592 | result += polyCoef[i] * powf(x, N - 2 - i) * (float)(N - 1 - i); 593 | } 594 | return result; 595 | } 596 | 597 | float polyDiff2Eval(float polyCoef[], float x, int N) 598 | { 599 | // Evaluate the polynomial's 2nd derivative given by polyCoef at variable x. 600 | // The order of polynomial is N. 601 | float result = 0; 602 | for (int i = 0; i < N - 2; i++) 603 | { 604 | result += polyCoef[i] * powf(x, N - 3 - i) * (float)(N - 1 - i) * (float)(N - 2 - i); 605 | } 606 | return result; 607 | } 608 | 609 | float polyDiff3Eval(float polyCoef[], float x, int N) 610 | { 611 | // Evaluate the polynomial's 3rd derivative given by polyCoef at variable x. 612 | // The order of polynomial is N. 613 | float result = 0; 614 | for (int i = 0; i < N - 3; i++) 615 | { 616 | result += polyCoef[i] * powf(x, N - 4 - i) * (float)(N - 1 - i) * (float)(N - 2 - i) * (float)(N - 3 - i); 617 | } 618 | return result; 619 | } 620 | 621 | float polyDiff4Eval(float polyCoef[], float x, int N) 622 | { 623 | // Evaluate the polynomial's 4th derivative given by polyCoef at variable x. 624 | // The order of polynomial is N. 625 | float result = 0; 626 | for (int i = 0; i < N - 4; i++) 627 | { 628 | result += polyCoef[i] * powf(x, N - 5 - i) * (float)(N - 1 - i) * (float)(N - 2 - i) * (float)(N - 3 - i) * (float)(N - 4 - i); 629 | } 630 | return result; 631 | } 632 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/mode.cpp: -------------------------------------------------------------------------------- 1 | #include "Copter.h" 2 | 3 | /* 4 | * High level calls to set and update flight modes logic for individual 5 | * flight modes is in control_acro.cpp, control_stabilize.cpp, etc 6 | */ 7 | 8 | /* 9 | constructor for Mode object 10 | */ 11 | Mode::Mode(void) : 12 | g(copter.g), 13 | g2(copter.g2), 14 | wp_nav(copter.wp_nav), 15 | loiter_nav(copter.loiter_nav), 16 | pos_control(copter.pos_control), 17 | inertial_nav(copter.inertial_nav), 18 | ahrs(copter.ahrs), 19 | attitude_control(copter.attitude_control), 20 | motors(copter.motors), 21 | channel_roll(copter.channel_roll), 22 | channel_pitch(copter.channel_pitch), 23 | channel_throttle(copter.channel_throttle), 24 | channel_yaw(copter.channel_yaw), 25 | G_Dt(copter.G_Dt) 26 | { }; 27 | 28 | // return the static controller object corresponding to supplied mode 29 | Mode *Copter::mode_from_mode_num(const Mode::Number mode) 30 | { 31 | Mode *ret = nullptr; 32 | 33 | switch (mode) { 34 | #if MODE_ACRO_ENABLED == ENABLED 35 | case Mode::Number::ACRO: 36 | ret = &mode_acro; 37 | break; 38 | #endif 39 | 40 | case Mode::Number::STABILIZE: 41 | ret = &mode_stabilize; 42 | break; 43 | 44 | case Mode::Number::ALT_HOLD: 45 | ret = &mode_althold; 46 | break; 47 | 48 | #if MODE_AUTO_ENABLED == ENABLED 49 | case Mode::Number::AUTO: 50 | ret = &mode_auto; 51 | break; 52 | #endif 53 | 54 | #if MODE_CIRCLE_ENABLED == ENABLED 55 | case Mode::Number::CIRCLE: 56 | ret = &mode_circle; 57 | break; 58 | #endif 59 | 60 | #if MODE_LOITER_ENABLED == ENABLED 61 | case Mode::Number::LOITER: 62 | ret = &mode_loiter; 63 | break; 64 | #endif 65 | 66 | #if MODE_GUIDED_ENABLED == ENABLED 67 | case Mode::Number::GUIDED: 68 | ret = &mode_guided; 69 | break; 70 | #endif 71 | 72 | case Mode::Number::LAND: 73 | ret = &mode_land; 74 | break; 75 | 76 | #if MODE_RTL_ENABLED == ENABLED 77 | case Mode::Number::RTL: 78 | ret = &mode_rtl; 79 | break; 80 | #endif 81 | 82 | #if MODE_DRIFT_ENABLED == ENABLED 83 | case Mode::Number::DRIFT: 84 | ret = &mode_drift; 85 | break; 86 | #endif 87 | 88 | #if MODE_SPORT_ENABLED == ENABLED 89 | case Mode::Number::SPORT: 90 | ret = &mode_sport; 91 | break; 92 | #endif 93 | 94 | #if MODE_FLIP_ENABLED == ENABLED 95 | case Mode::Number::FLIP: 96 | ret = &mode_flip; 97 | break; 98 | #endif 99 | 100 | #if AUTOTUNE_ENABLED == ENABLED 101 | case Mode::Number::AUTOTUNE: 102 | ret = &mode_autotune; 103 | break; 104 | #endif 105 | 106 | #if MODE_POSHOLD_ENABLED == ENABLED 107 | case Mode::Number::POSHOLD: 108 | ret = &mode_poshold; 109 | break; 110 | #endif 111 | 112 | #if MODE_BRAKE_ENABLED == ENABLED 113 | case Mode::Number::BRAKE: 114 | ret = &mode_brake; 115 | break; 116 | #endif 117 | 118 | #if MODE_THROW_ENABLED == ENABLED 119 | case Mode::Number::THROW: 120 | ret = &mode_throw; 121 | break; 122 | #endif 123 | 124 | #if HAL_ADSB_ENABLED 125 | case Mode::Number::AVOID_ADSB: 126 | ret = &mode_avoid_adsb; 127 | break; 128 | #endif 129 | 130 | #if MODE_GUIDED_NOGPS_ENABLED == ENABLED 131 | case Mode::Number::GUIDED_NOGPS: 132 | ret = &mode_guided_nogps; 133 | break; 134 | #endif 135 | 136 | #if MODE_SMARTRTL_ENABLED == ENABLED 137 | case Mode::Number::SMART_RTL: 138 | ret = &mode_smartrtl; 139 | break; 140 | #endif 141 | 142 | #if MODE_FLOWHOLD_ENABLED == ENABLED 143 | case Mode::Number::FLOWHOLD: 144 | ret = (Mode *)g2.mode_flowhold_ptr; 145 | break; 146 | #endif 147 | 148 | #if MODE_FOLLOW_ENABLED == ENABLED 149 | case Mode::Number::FOLLOW: 150 | ret = &mode_follow; 151 | break; 152 | #endif 153 | 154 | #if MODE_ZIGZAG_ENABLED == ENABLED 155 | case Mode::Number::ZIGZAG: 156 | ret = &mode_zigzag; 157 | break; 158 | #endif 159 | 160 | #if MODE_SYSTEMID_ENABLED == ENABLED 161 | case Mode::Number::SYSTEMID: 162 | ret = (Mode *)g2.mode_systemid_ptr; 163 | break; 164 | #endif 165 | 166 | #if MODE_AUTOROTATE_ENABLED == ENABLED 167 | case Mode::Number::AUTOROTATE: 168 | ret = &mode_autorotate; 169 | break; 170 | #endif 171 | 172 | #if MODE_TURTLE_ENABLED == ENABLED 173 | case Mode::Number::TURTLE: 174 | ret = &mode_turtle; 175 | break; 176 | #endif 177 | 178 | #if MODE_ADAPTIVE_ENABLED == ENABLED 179 | case Mode::Number::ADAPTIVE: 180 | ret = &mode_adaptive; 181 | break; 182 | #endif 183 | 184 | default: 185 | break; 186 | } 187 | 188 | return ret; 189 | } 190 | 191 | 192 | // called when an attempt to change into a mode is unsuccessful: 193 | void Copter::mode_change_failed(const Mode *mode, const char *reason) 194 | { 195 | gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason); 196 | AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number())); 197 | // make sad noise 198 | if (copter.ap.initialised) { 199 | AP_Notify::events.user_mode_change_failed = 1; 200 | } 201 | } 202 | 203 | // set_mode - change flight mode and perform any necessary initialisation 204 | // optional force parameter used to force the flight mode change (used only first time mode is set) 205 | // returns true if mode was successfully set 206 | // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately 207 | bool Copter::set_mode(Mode::Number mode, ModeReason reason) 208 | { 209 | // update last reason 210 | const ModeReason last_reason = _last_reason; 211 | _last_reason = reason; 212 | 213 | // return immediately if we are already in the desired mode 214 | if (mode == flightmode->mode_number()) { 215 | control_mode_reason = reason; 216 | // set yaw rate time constant during autopilot startup 217 | if (reason == ModeReason::INITIALISED && mode == Mode::Number::STABILIZE) { 218 | attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc()); 219 | } 220 | // make happy noise 221 | if (copter.ap.initialised && (reason != last_reason)) { 222 | AP_Notify::events.user_mode_change = 1; 223 | } 224 | return true; 225 | } 226 | 227 | #if MODE_AUTO_ENABLED == ENABLED 228 | if (mode == Mode::Number::AUTO_RTL) { 229 | // Special case for AUTO RTL, not a true mode, just AUTO in disguise 230 | return mode_auto.jump_to_landing_sequence_auto_RTL(reason); 231 | } 232 | #endif 233 | 234 | Mode *new_flightmode = mode_from_mode_num(mode); 235 | if (new_flightmode == nullptr) { 236 | notify_no_such_mode((uint8_t)mode); 237 | return false; 238 | } 239 | 240 | bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform 241 | 242 | #if FRAME_CONFIG == HELI_FRAME 243 | // do not allow helis to enter a non-manual throttle mode if the 244 | // rotor runup is not complete 245 | if (!ignore_checks && !new_flightmode->has_manual_throttle() && 246 | (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) { 247 | #if MODE_AUTOROTATE_ENABLED == ENABLED 248 | //if the mode being exited is the autorotation mode allow mode change despite rotor not being at 249 | //full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes 250 | bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate); 251 | #else 252 | bool in_autorotation_check = false; 253 | #endif 254 | 255 | if (!in_autorotation_check) { 256 | mode_change_failed(new_flightmode, "runup not complete"); 257 | return false; 258 | } 259 | } 260 | #endif 261 | 262 | #if FRAME_CONFIG != HELI_FRAME 263 | // ensure vehicle doesn't leap off the ground if a user switches 264 | // into a manual throttle mode from a non-manual-throttle mode 265 | // (e.g. user arms in guided, raises throttle to 1300 (not enough to 266 | // trigger auto takeoff), then switches into manual): 267 | bool user_throttle = new_flightmode->has_manual_throttle(); 268 | #if MODE_DRIFT_ENABLED == ENABLED 269 | if (new_flightmode == &mode_drift) { 270 | user_throttle = true; 271 | } 272 | #endif 273 | if (!ignore_checks && 274 | ap.land_complete && 275 | user_throttle && 276 | !copter.flightmode->has_manual_throttle() && 277 | new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) { 278 | mode_change_failed(new_flightmode, "throttle too high"); 279 | return false; 280 | } 281 | #endif 282 | 283 | if (!ignore_checks && 284 | new_flightmode->requires_GPS() && 285 | !copter.position_ok()) { 286 | mode_change_failed(new_flightmode, "requires position"); 287 | return false; 288 | } 289 | 290 | // check for valid altitude if old mode did not require it but new one does 291 | // we only want to stop changing modes if it could make things worse 292 | if (!ignore_checks && 293 | !copter.ekf_alt_ok() && 294 | flightmode->has_manual_throttle() && 295 | !new_flightmode->has_manual_throttle()) { 296 | mode_change_failed(new_flightmode, "need alt estimate"); 297 | return false; 298 | } 299 | 300 | if (!new_flightmode->init(ignore_checks)) { 301 | mode_change_failed(new_flightmode, "init failed"); 302 | return false; 303 | } 304 | 305 | // perform any cleanup required by previous flight mode 306 | exit_mode(flightmode, new_flightmode); 307 | 308 | // store previous flight mode (only used by tradeheli's autorotation) 309 | prev_control_mode = flightmode->mode_number(); 310 | 311 | // update flight mode 312 | flightmode = new_flightmode; 313 | control_mode_reason = reason; 314 | logger.Write_Mode((uint8_t)flightmode->mode_number(), reason); 315 | gcs().send_message(MSG_HEARTBEAT); 316 | 317 | #if HAL_ADSB_ENABLED 318 | adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED)); 319 | #endif 320 | 321 | #if AP_FENCE_ENABLED 322 | // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover 323 | // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) 324 | // but it should be harmless to disable the fence temporarily in these situations as well 325 | fence.manual_recovery_start(); 326 | #endif 327 | 328 | #if AP_CAMERA_ENABLED 329 | camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO); 330 | #endif 331 | 332 | // set rate shaping time constants 333 | #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED 334 | attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc()); 335 | #endif 336 | attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc()); 337 | #if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED 338 | if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) { 339 | attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc()); 340 | } 341 | #endif 342 | 343 | // update notify object 344 | notify_flight_mode(); 345 | 346 | // make happy noise 347 | if (copter.ap.initialised) { 348 | AP_Notify::events.user_mode_change = 1; 349 | } 350 | 351 | // return success 352 | return true; 353 | } 354 | 355 | bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason) 356 | { 357 | static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); 358 | #ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE 359 | if (reason == ModeReason::GCS_COMMAND && copter.failsafe.radio) { 360 | // don't allow mode changes while in radio failsafe 361 | return false; 362 | } 363 | #endif 364 | return copter.set_mode(static_cast(new_mode), reason); 365 | } 366 | 367 | // update_flight_mode - calls the appropriate attitude controllers based on flight mode 368 | // called at 100hz or more 369 | void Copter::update_flight_mode() 370 | { 371 | surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used 372 | 373 | flightmode->run(); 374 | } 375 | 376 | // exit_mode - high level call to organise cleanup as a flight mode is exited 377 | void Copter::exit_mode(Mode *&old_flightmode, 378 | Mode *&new_flightmode) 379 | { 380 | // smooth throttle transition when switching from manual to automatic flight modes 381 | if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) { 382 | // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle 383 | set_accel_throttle_I_from_pilot_throttle(); 384 | } 385 | 386 | // cancel any takeoffs in progress 387 | old_flightmode->takeoff_stop(); 388 | 389 | // perform cleanup required for each flight mode 390 | old_flightmode->exit(); 391 | 392 | #if FRAME_CONFIG == HELI_FRAME 393 | // firmly reset the flybar passthrough to false when exiting acro mode. 394 | if (old_flightmode == &mode_acro) { 395 | attitude_control->use_flybar_passthrough(false, false); 396 | motors->set_acro_tail(false); 397 | } 398 | 399 | // if we are changing from a mode that did not use manual throttle, 400 | // stab col ramp value should be pre-loaded to the correct value to avoid a twitch 401 | // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes 402 | if (!old_flightmode->has_manual_throttle()){ 403 | if (new_flightmode == &mode_stabilize){ 404 | input_manager.set_stab_col_ramp(1.0); 405 | } else if (new_flightmode == &mode_acro){ 406 | input_manager.set_stab_col_ramp(0.0); 407 | } 408 | } 409 | #endif //HELI_FRAME 410 | } 411 | 412 | // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device 413 | void Copter::notify_flight_mode() { 414 | AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); 415 | AP_Notify::flags.flight_mode = (uint8_t)flightmode->mode_number(); 416 | notify.set_flight_mode_str(flightmode->name4()); 417 | } 418 | 419 | // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle 420 | // returns desired angle in centi-degrees 421 | void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const 422 | { 423 | // throttle failsafe check 424 | if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { 425 | roll_out_cd = 0.0; 426 | pitch_out_cd = 0.0; 427 | return; 428 | } 429 | 430 | //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command 431 | float roll_out_deg; 432 | float pitch_out_deg; 433 | rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01, angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg); 434 | 435 | // Convert to centi-degrees 436 | roll_out_cd = roll_out_deg * 100.0; 437 | pitch_out_cd = pitch_out_deg * 100.0; 438 | } 439 | 440 | // transform pilot's roll or pitch input into a desired velocity 441 | Vector2f Mode::get_pilot_desired_velocity(float vel_max) const 442 | { 443 | Vector2f vel; 444 | 445 | // throttle failsafe check 446 | if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { 447 | return vel; 448 | } 449 | // fetch roll and pitch inputs 450 | float roll_out = channel_roll->get_control_in(); 451 | float pitch_out = channel_pitch->get_control_in(); 452 | 453 | // convert roll and pitch inputs to -1 to +1 range 454 | float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX; 455 | roll_out *= scaler; 456 | pitch_out *= scaler; 457 | 458 | // convert roll and pitch inputs into velocity in NE frame 459 | vel = Vector2f(-pitch_out, roll_out); 460 | if (vel.is_zero()) { 461 | return vel; 462 | } 463 | copter.rotate_body_frame_to_NE(vel.x, vel.y); 464 | 465 | // Transform square input range to circular output 466 | // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input 467 | Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y)); 468 | // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max 469 | vel *= vel_max / vel_scaler.length(); 470 | return vel; 471 | } 472 | 473 | bool Mode::_TakeOff::triggered(const float target_climb_rate) const 474 | { 475 | if (!copter.ap.land_complete) { 476 | // can't take off if we're already flying 477 | return false; 478 | } 479 | if (target_climb_rate <= 0.0f) { 480 | // can't takeoff unless we want to go up... 481 | return false; 482 | } 483 | 484 | if (copter.motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) { 485 | // hold aircraft on the ground until rotor speed runup has finished 486 | return false; 487 | } 488 | 489 | return true; 490 | } 491 | 492 | bool Mode::is_disarmed_or_landed() const 493 | { 494 | if (!motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) { 495 | return true; 496 | } 497 | return false; 498 | } 499 | 500 | void Mode::zero_throttle_and_relax_ac(bool spool_up) 501 | { 502 | if (spool_up) { 503 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 504 | } else { 505 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 506 | } 507 | attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); 508 | attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); 509 | } 510 | 511 | void Mode::zero_throttle_and_hold_attitude() 512 | { 513 | // run attitude controller 514 | attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); 515 | attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); 516 | } 517 | 518 | // handle situations where the vehicle is on the ground waiting for takeoff 519 | // force_throttle_unlimited should be true in cases where we want to keep the motors spooled up 520 | // (instead of spooling down to ground idle). This is required for tradheli's in Guided and Auto 521 | // where we always want the motor spooled up in Guided or Auto mode. Tradheli's main rotor stops 522 | // when spooled down to ground idle. 523 | // ultimately it forces the motor interlock to be obeyed in auto and guided modes when on the ground. 524 | void Mode::make_safe_ground_handling(bool force_throttle_unlimited) 525 | { 526 | if (force_throttle_unlimited) { 527 | // keep rotors turning 528 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 529 | } else { 530 | // spool down to ground idle 531 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 532 | } 533 | 534 | // aircraft is landed, integrator terms must be reset regardless of spool state 535 | attitude_control->reset_rate_controller_I_terms_smoothly(); 536 | 537 | switch (motors->get_spool_state()) { 538 | case AP_Motors::SpoolState::SHUT_DOWN: 539 | case AP_Motors::SpoolState::GROUND_IDLE: 540 | // reset yaw targets and rates during idle states 541 | attitude_control->reset_yaw_target_and_rate(); 542 | break; 543 | case AP_Motors::SpoolState::SPOOLING_UP: 544 | case AP_Motors::SpoolState::THROTTLE_UNLIMITED: 545 | case AP_Motors::SpoolState::SPOOLING_DOWN: 546 | // while transitioning though active states continue to operate normally 547 | break; 548 | } 549 | 550 | pos_control->relax_velocity_controller_xy(); 551 | pos_control->update_xy_controller(); 552 | pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero 553 | pos_control->update_z_controller(); 554 | // we may need to move this out 555 | attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); 556 | } 557 | 558 | /* 559 | get a height above ground estimate for landing 560 | */ 561 | int32_t Mode::get_alt_above_ground_cm(void) 562 | { 563 | int32_t alt_above_ground_cm; 564 | if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) { 565 | return alt_above_ground_cm; 566 | } 567 | if (!pos_control->is_active_xy()) { 568 | return copter.current_loc.alt; 569 | } 570 | if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) { 571 | return alt_above_ground_cm; 572 | } 573 | 574 | // Assume the Earth is flat: 575 | return copter.current_loc.alt; 576 | } 577 | 578 | void Mode::land_run_vertical_control(bool pause_descent) 579 | { 580 | float cmb_rate = 0; 581 | bool ignore_descent_limit = false; 582 | if (!pause_descent) { 583 | 584 | // do not ignore limits until we have slowed down for landing 585 | ignore_descent_limit = (MAX(g2.land_alt_low,100) > get_alt_above_ground_cm()) || copter.ap.land_complete_maybe; 586 | 587 | float max_land_descent_velocity; 588 | if (g.land_speed_high > 0) { 589 | max_land_descent_velocity = -g.land_speed_high; 590 | } else { 591 | max_land_descent_velocity = pos_control->get_max_speed_down_cms(); 592 | } 593 | 594 | // Don't speed up for landing. 595 | max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); 596 | 597 | // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. 598 | cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); 599 | 600 | // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. 601 | cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); 602 | 603 | #if PRECISION_LANDING == ENABLED 604 | const bool navigating = pos_control->is_active_xy(); 605 | bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; 606 | 607 | if (doing_precision_landing) { 608 | // prec landing is active 609 | Vector2f target_pos; 610 | float target_error_cm = 0.0f; 611 | if (copter.precland.get_target_position_cm(target_pos)) { 612 | const Vector2f current_pos = inertial_nav.get_position_xy_cm(); 613 | // target is this many cm away from the vehicle 614 | target_error_cm = (target_pos - current_pos).length(); 615 | } 616 | // check if we should descend or not 617 | const float max_horiz_pos_error_cm = copter.precland.get_max_xy_error_before_descending_cm(); 618 | Vector3f target_pos_meas; 619 | copter.precland.get_target_position_measurement_cm(target_pos_meas); 620 | if (target_error_cm > max_horiz_pos_error_cm && !is_zero(max_horiz_pos_error_cm)) { 621 | // doing precland but too far away from the obstacle 622 | // do not descend 623 | cmb_rate = 0.0f; 624 | } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f) { 625 | // very close to the ground and doing prec land, lets slow down to make sure we land on target 626 | // compute desired descent velocity 627 | const float precland_acceptable_error_cm = 15.0f; 628 | const float precland_min_descent_speed_cms = 10.0f; 629 | const float max_descent_speed_cms = abs(g.land_speed)*0.5f; 630 | const float land_slowdown = MAX(0.0f, target_error_cm*(max_descent_speed_cms/precland_acceptable_error_cm)); 631 | cmb_rate = MIN(-precland_min_descent_speed_cms, -max_descent_speed_cms+land_slowdown); 632 | } 633 | } 634 | #endif 635 | } 636 | 637 | // update altitude target and call position controller 638 | pos_control->land_at_climb_rate_cm(cmb_rate, ignore_descent_limit); 639 | pos_control->update_z_controller(); 640 | } 641 | 642 | void Mode::land_run_horizontal_control() 643 | { 644 | Vector2f vel_correction; 645 | 646 | // relax loiter target if we might be landed 647 | if (copter.ap.land_complete_maybe) { 648 | pos_control->soften_for_landing_xy(); 649 | } 650 | 651 | // process pilot inputs 652 | if (!copter.failsafe.radio) { 653 | if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ 654 | AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); 655 | // exit land if throttle is high 656 | if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { 657 | set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); 658 | } 659 | } 660 | 661 | if (g.land_repositioning) { 662 | // apply SIMPLE mode transform to pilot inputs 663 | update_simple_mode(); 664 | 665 | // convert pilot input to reposition velocity 666 | // use half maximum acceleration as the maximum velocity to ensure aircraft will 667 | // stop from full reposition speed in less than 1 second. 668 | const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5; 669 | vel_correction = get_pilot_desired_velocity(max_pilot_vel); 670 | 671 | // record if pilot has overridden roll or pitch 672 | if (!vel_correction.is_zero()) { 673 | if (!copter.ap.land_repo_active) { 674 | AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); 675 | } 676 | copter.ap.land_repo_active = true; 677 | #if PRECISION_LANDING == ENABLED 678 | } else { 679 | // no override right now, check if we should allow precland 680 | if (copter.precland.allow_precland_after_reposition()) { 681 | copter.ap.land_repo_active = false; 682 | } 683 | #endif 684 | } 685 | } 686 | } 687 | 688 | // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle 689 | copter.ap.prec_land_active = false; 690 | #if PRECISION_LANDING == ENABLED 691 | copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); 692 | // run precision landing 693 | if (copter.ap.prec_land_active) { 694 | Vector2f target_pos, target_vel; 695 | if (!copter.precland.get_target_position_cm(target_pos)) { 696 | target_pos = inertial_nav.get_position_xy_cm(); 697 | } 698 | // get the velocity of the target 699 | copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel); 700 | 701 | Vector2f zero; 702 | Vector2p landing_pos = target_pos.topostype(); 703 | // target vel will remain zero if landing target is stationary 704 | pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero); 705 | } 706 | #endif 707 | 708 | if (!copter.ap.prec_land_active) { 709 | Vector2f accel; 710 | pos_control->input_vel_accel_xy(vel_correction, accel); 711 | } 712 | 713 | // run pos controller 714 | pos_control->update_xy_controller(); 715 | Vector3f thrust_vector = pos_control->get_thrust_vector(); 716 | 717 | if (g2.wp_navalt_min > 0) { 718 | // user has requested an altitude below which navigation 719 | // attitude is limited. This is used to prevent commanded roll 720 | // over on landing, which particularly affects helicopters if 721 | // there is any position estimate drift after touchdown. We 722 | // limit attitude to 7 degrees below this limit and linearly 723 | // interpolate for 1m above that 724 | const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), 725 | g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); 726 | const float thrust_vector_max = sinf(radians(attitude_limit_cd * 0.01f)) * GRAVITY_MSS * 100.0f; 727 | const float thrust_vector_mag = thrust_vector.xy().length(); 728 | if (thrust_vector_mag > thrust_vector_max) { 729 | float ratio = thrust_vector_max / thrust_vector_mag; 730 | thrust_vector.x *= ratio; 731 | thrust_vector.y *= ratio; 732 | 733 | // tell position controller we are applying an external limit 734 | pos_control->set_externally_limited_xy(); 735 | } 736 | } 737 | 738 | // call attitude controller 739 | attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading()); 740 | 741 | } 742 | 743 | // run normal or precision landing (if enabled) 744 | // pause_descent is true if vehicle should not descend 745 | void Mode::land_run_normal_or_precland(bool pause_descent) 746 | { 747 | #if PRECISION_LANDING == ENABLED 748 | if (pause_descent || !copter.precland.enabled()) { 749 | // we don't want to start descending immediately or prec land is disabled 750 | // in both cases just run simple land controllers 751 | land_run_horiz_and_vert_control(pause_descent); 752 | } else { 753 | // prec land is enabled and we have not paused descent 754 | // the state machine takes care of the entire prec landing procedure 755 | precland_run(); 756 | } 757 | #else 758 | land_run_horiz_and_vert_control(pause_descent); 759 | #endif 760 | } 761 | 762 | #if PRECISION_LANDING == ENABLED 763 | // Go towards a position commanded by prec land state machine in order to retry landing 764 | // The passed in location is expected to be NED and in m 765 | void Mode::precland_retry_position(const Vector3f &retry_pos) 766 | { 767 | if (!copter.failsafe.radio) { 768 | if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ 769 | AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); 770 | // exit land if throttle is high 771 | if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { 772 | set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); 773 | } 774 | } 775 | 776 | // allow user to take control during repositioning. Note: copied from land_run_horizontal_control() 777 | // To-Do: this code exists at several different places in slightly different forms and that should be fixed 778 | if (g.land_repositioning) { 779 | float target_roll = 0.0f; 780 | float target_pitch = 0.0f; 781 | // convert pilot input to lean angles 782 | get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); 783 | 784 | // record if pilot has overridden roll or pitch 785 | if (!is_zero(target_roll) || !is_zero(target_pitch)) { 786 | if (!copter.ap.land_repo_active) { 787 | AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); 788 | } 789 | // this flag will be checked by prec land state machine later and any further landing retires will be cancelled 790 | copter.ap.land_repo_active = true; 791 | } 792 | } 793 | } 794 | 795 | Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f}; 796 | // pos controller expects input in NEU cm's 797 | retry_pos_NEU = retry_pos_NEU * 100.0f; 798 | pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f); 799 | 800 | // run position controllers 801 | pos_control->update_xy_controller(); 802 | pos_control->update_z_controller(); 803 | 804 | // call attitude controller 805 | attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); 806 | 807 | } 808 | 809 | // Run precland statemachine. This function should be called from any mode that wants to do precision landing. 810 | // This handles everything from prec landing, to prec landing failures, to retries and failsafe measures 811 | void Mode::precland_run() 812 | { 813 | // if user is taking control, we will not run the statemachine, and simply land (may or may not be on target) 814 | if (!copter.ap.land_repo_active) { 815 | // This will get updated later to a retry pos if needed 816 | Vector3f retry_pos; 817 | 818 | switch (copter.precland_statemachine.update(retry_pos)) { 819 | case AC_PrecLand_StateMachine::Status::RETRYING: 820 | // we want to retry landing by going to another position 821 | precland_retry_position(retry_pos); 822 | break; 823 | 824 | case AC_PrecLand_StateMachine::Status::FAILSAFE: { 825 | // we have hit a failsafe. Failsafe can only mean two things, we either want to stop permanently till user takes over or land 826 | switch (copter.precland_statemachine.get_failsafe_actions()) { 827 | case AC_PrecLand_StateMachine::FailSafeAction::DESCEND: 828 | // descend normally, prec land target is definitely not in sight 829 | land_run_horiz_and_vert_control(); 830 | break; 831 | case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS: 832 | // sending "true" in this argument will stop the descend 833 | land_run_horiz_and_vert_control(true); 834 | break; 835 | } 836 | break; 837 | } 838 | case AC_PrecLand_StateMachine::Status::ERROR: 839 | // should never happen, is certainly a bug. Report then descend 840 | INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); 841 | FALLTHROUGH; 842 | case AC_PrecLand_StateMachine::Status::DESCEND: 843 | // run land controller. This will descend towards the target if prec land target is in sight 844 | // else it will just descend vertically 845 | land_run_horiz_and_vert_control(); 846 | break; 847 | } 848 | } else { 849 | // just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures 850 | land_run_horiz_and_vert_control(); 851 | } 852 | } 853 | #endif 854 | 855 | float Mode::throttle_hover() const 856 | { 857 | return motors->get_throttle_hover(); 858 | } 859 | 860 | // transform pilot's manual throttle input to make hover throttle mid stick 861 | // used only for manual throttle modes 862 | // thr_mid should be in the range 0 to 1 863 | // returns throttle output 0 to 1 864 | float Mode::get_pilot_desired_throttle() const 865 | { 866 | const float thr_mid = throttle_hover(); 867 | int16_t throttle_control = channel_throttle->get_control_in(); 868 | 869 | int16_t mid_stick = copter.get_throttle_mid(); 870 | // protect against unlikely divide by zero 871 | if (mid_stick <= 0) { 872 | mid_stick = 500; 873 | } 874 | 875 | // ensure reasonable throttle values 876 | throttle_control = constrain_int16(throttle_control,0,1000); 877 | 878 | // calculate normalised throttle input 879 | float throttle_in; 880 | if (throttle_control < mid_stick) { 881 | throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick; 882 | } else { 883 | throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick); 884 | } 885 | 886 | const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f); 887 | // calculate the output throttle using the given expo function 888 | float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in; 889 | return throttle_out; 890 | } 891 | 892 | float Mode::get_avoidance_adjusted_climbrate(float target_rate) 893 | { 894 | #if AC_AVOID_ENABLED == ENABLED 895 | AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), target_rate, G_Dt); 896 | return target_rate; 897 | #else 898 | return target_rate; 899 | #endif 900 | } 901 | 902 | // send output to the motors, can be overridden by subclasses 903 | void Mode::output_to_motors() 904 | { 905 | motors->output(); 906 | } 907 | 908 | Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms) 909 | { 910 | // Alt Hold State Machine Determination 911 | if (!motors->armed()) { 912 | // the aircraft should moved to a shut down state 913 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); 914 | 915 | // transition through states as aircraft spools down 916 | switch (motors->get_spool_state()) { 917 | 918 | case AP_Motors::SpoolState::SHUT_DOWN: 919 | return AltHold_MotorStopped; 920 | 921 | case AP_Motors::SpoolState::GROUND_IDLE: 922 | return AltHold_Landed_Ground_Idle; 923 | 924 | default: 925 | return AltHold_Landed_Pre_Takeoff; 926 | } 927 | 928 | } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) { 929 | // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED 930 | // the aircraft should progress through the take off procedure 931 | return AltHold_Takeoff; 932 | 933 | } else if (!copter.ap.auto_armed || copter.ap.land_complete) { 934 | // the aircraft is armed and landed 935 | if (target_climb_rate_cms < 0.0f && !copter.ap.using_interlock) { 936 | // the aircraft should move to a ground idle state 937 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 938 | 939 | } else { 940 | // the aircraft should prepare for imminent take off 941 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 942 | } 943 | 944 | if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { 945 | // the aircraft is waiting in ground idle 946 | return AltHold_Landed_Ground_Idle; 947 | 948 | } else { 949 | // the aircraft can leave the ground at any time 950 | return AltHold_Landed_Pre_Takeoff; 951 | } 952 | 953 | } else { 954 | // the aircraft is in a flying state 955 | motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 956 | return AltHold_Flying; 957 | } 958 | } 959 | 960 | // transform pilot's yaw input into a desired yaw rate 961 | // returns desired yaw rate in centi-degrees per second 962 | float Mode::get_pilot_desired_yaw_rate(float yaw_in) 963 | { 964 | // throttle failsafe check 965 | if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { 966 | return 0.0f; 967 | } 968 | 969 | // convert pilot input to the desired yaw rate 970 | return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo()); 971 | } 972 | 973 | // pass-through functions to reduce code churn on conversion; 974 | // these are candidates for moving into the Mode base 975 | // class. 976 | float Mode::get_pilot_desired_climb_rate(float throttle_control) 977 | { 978 | return copter.get_pilot_desired_climb_rate(throttle_control); 979 | } 980 | 981 | float Mode::get_non_takeoff_throttle() 982 | { 983 | return copter.get_non_takeoff_throttle(); 984 | } 985 | 986 | void Mode::update_simple_mode(void) { 987 | copter.update_simple_mode(); 988 | } 989 | 990 | bool Mode::set_mode(Mode::Number mode, ModeReason reason) 991 | { 992 | return copter.set_mode(mode, reason); 993 | } 994 | 995 | void Mode::set_land_complete(bool b) 996 | { 997 | return copter.set_land_complete(b); 998 | } 999 | 1000 | GCS_Copter &Mode::gcs() 1001 | { 1002 | return copter.gcs(); 1003 | } 1004 | 1005 | uint16_t Mode::get_pilot_speed_dn() 1006 | { 1007 | return copter.get_pilot_speed_dn(); 1008 | } 1009 | -------------------------------------------------------------------------------- /L1AC_customization/ArduCopter/Copter.h: -------------------------------------------------------------------------------- 1 | /* 2 | This program is free software: you can redistribute it and/or modify 3 | it under the terms of the GNU General Public License as published by 4 | the Free Software Foundation, either version 3 of the License, or 5 | (at your option) any later version. 6 | 7 | This program is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | GNU General Public License for more details. 11 | 12 | You should have received a copy of the GNU General Public License 13 | along with this program. If not, see . 14 | */ 15 | #pragma once 16 | /* 17 | This is the main Copter class 18 | */ 19 | 20 | //////////////////////////////////////////////////////////////////////////////// 21 | // Header includes 22 | //////////////////////////////////////////////////////////////////////////////// 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | // Common dependencies 31 | #include // Common definitions and utility routines for the ArduPilot libraries 32 | #include // Library having the implementation of location class 33 | #include // A system for managing and storing variables that are of general interest to the system. 34 | #include // library for Management for hal.storage to allow for backwards compatible mapping of storage offsets to available storage 35 | 36 | // Application dependencies 37 | #include // ArduPilot Mega Flash Memory Library 38 | #include // ArduPilot Mega Vector/Matrix math Library 39 | #include // interface and maths for accelerometer calibration 40 | #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library 41 | #include // AHRS (Attitude Heading Reference System) interface library for ArduPilot 42 | #include // Mission command library 43 | #include // Mission command change detection library 44 | #include // Attitude control library 45 | #include // 6DoF Attitude control library 46 | #include // Attitude control library for traditional helicopter 47 | #include // Position control library 48 | #include // Command model library 49 | #include // AP Motors library 50 | #include // statistics library 51 | #include // Filter library 52 | #include // needed for AHRS build 53 | #include // inertial navigation library 54 | #include // ArduCopter waypoint navigation library 55 | #include // ArduCopter Loiter Mode Library 56 | #include // circle navigation library 57 | #include // ArduPilot Mega Declination Helper Library 58 | #include // RC input mapping library 59 | #include // Battery monitor library 60 | #include // Landing Gear library 61 | #include // Pilot input handling library 62 | #include // Heli specific pilot input handling library 63 | #include // ArduPilot motor arming library 64 | #include // ArduPilot Smart Return To Launch Mode (SRTL) library 65 | #include // temperature calibration library 66 | #include // ArduCopter autotune library. support for autotune of multirotors. 67 | #include // ArduCopter autotune library. support for autotune of helicopters. 68 | #include // ArduPilot parachute release library 69 | #include // Crop sprayer library 70 | #include // ADS-B RF based collision avoidance module library 71 | #include // ArduPilot proximity sensor library 72 | #include 73 | #include 74 | 75 | // Configuration 76 | #include "defines.h" 77 | #include "config.h" 78 | 79 | #if FRAME_CONFIG == HELI_FRAME 80 | #define AC_AttitudeControl_t AC_AttitudeControl_Heli 81 | #else 82 | #define AC_AttitudeControl_t AC_AttitudeControl_Multi 83 | #endif 84 | 85 | #if FRAME_CONFIG == HELI_FRAME 86 | #define MOTOR_CLASS AP_MotorsHeli 87 | #else 88 | #define MOTOR_CLASS AP_MotorsMulticopter 89 | #endif 90 | 91 | #if MODE_AUTOROTATE_ENABLED == ENABLED 92 | #include // Autorotation controllers 93 | #endif 94 | 95 | #include "RC_Channel.h" // RC Channel Library 96 | 97 | #include "GCS_Mavlink.h" 98 | #include "GCS_Copter.h" 99 | #include "AP_Rally.h" // Rally point library 100 | #include "AP_Arming.h" 101 | 102 | // libraries which are dependent on #defines in defines.h and/or config.h 103 | #if BEACON_ENABLED == ENABLED 104 | #include 105 | #endif 106 | 107 | #if AC_AVOID_ENABLED == ENABLED 108 | #include 109 | #endif 110 | #if AC_OAPATHPLANNER_ENABLED == ENABLED 111 | #include 112 | #include 113 | #endif 114 | #include 115 | #if AP_GRIPPER_ENABLED 116 | # include 117 | #endif 118 | #if PRECISION_LANDING == ENABLED 119 | # include 120 | # include 121 | #endif 122 | #if MODE_FOLLOW_ENABLED == ENABLED 123 | # include 124 | #endif 125 | #if AP_TERRAIN_AVAILABLE 126 | # include 127 | #endif 128 | #if RANGEFINDER_ENABLED == ENABLED 129 | # include 130 | #endif 131 | 132 | #include 133 | 134 | #include 135 | 136 | #if HAL_BUTTON_ENABLED 137 | # include 138 | #endif 139 | 140 | #if OSD_ENABLED || OSD_PARAM_ENABLED 141 | #include 142 | #endif 143 | 144 | #if ADVANCED_FAILSAFE == ENABLED 145 | # include "afs_copter.h" 146 | #endif 147 | #if TOY_MODE_ENABLED == ENABLED 148 | # include "toy_mode.h" 149 | #endif 150 | #if AP_WINCH_ENABLED 151 | # include 152 | #endif 153 | #include 154 | 155 | #if AP_SCRIPTING_ENABLED 156 | #include 157 | #endif 158 | 159 | #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED 160 | #include // Custom control library 161 | #endif 162 | 163 | #if AC_AVOID_ENABLED && !AP_FENCE_ENABLED 164 | #error AC_Avoidance relies on AP_FENCE_ENABLED which is disabled 165 | #endif 166 | 167 | #if AC_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED 168 | #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled 169 | #endif 170 | 171 | // Local modules 172 | #ifdef USER_PARAMS_ENABLED 173 | #include "UserParameters.h" 174 | #endif 175 | #include "Parameters.h" 176 | #if HAL_ADSB_ENABLED 177 | #include "avoidance_adsb.h" 178 | #endif 179 | 180 | #include "mode.h" 181 | 182 | class Copter : public AP_Vehicle { 183 | public: 184 | friend class GCS_MAVLINK_Copter; 185 | friend class GCS_Copter; 186 | friend class AP_Rally_Copter; 187 | friend class Parameters; 188 | friend class ParametersG2; 189 | friend class AP_Avoidance_Copter; 190 | 191 | #if ADVANCED_FAILSAFE == ENABLED 192 | friend class AP_AdvancedFailsafe_Copter; 193 | #endif 194 | friend class AP_Arming_Copter; 195 | friend class ToyMode; 196 | friend class RC_Channel_Copter; 197 | friend class RC_Channels_Copter; 198 | 199 | friend class AutoTune; 200 | 201 | friend class Mode; 202 | friend class ModeAcro; 203 | friend class ModeAcro_Heli; 204 | friend class ModeAltHold; 205 | friend class ModeAuto; 206 | friend class ModeAutoTune; 207 | friend class ModeAvoidADSB; 208 | friend class ModeBrake; 209 | friend class ModeCircle; 210 | friend class ModeDrift; 211 | friend class ModeFlip; 212 | friend class ModeFlowHold; 213 | friend class ModeFollow; 214 | friend class ModeGuided; 215 | friend class ModeLand; 216 | friend class ModeLoiter; 217 | friend class ModePosHold; 218 | friend class ModeRTL; 219 | friend class ModeSmartRTL; 220 | friend class ModeSport; 221 | friend class ModeStabilize; 222 | friend class ModeStabilize_Heli; 223 | friend class ModeSystemId; 224 | friend class ModeThrow; 225 | friend class ModeZigZag; 226 | friend class ModeAutorotate; 227 | friend class ModeTurtle; 228 | friend class ModeAdaptive; 229 | 230 | Copter(void); 231 | 232 | private: 233 | 234 | // key aircraft parameters passed to multiple libraries 235 | AP_MultiCopter aparm; 236 | 237 | // Global parameters are all contained within the 'g' class. 238 | Parameters g; 239 | ParametersG2 g2; 240 | 241 | // used to detect MAVLink acks from GCS to stop compassmot 242 | uint8_t command_ack_counter; 243 | 244 | // primary input control channels 245 | RC_Channel *channel_roll; 246 | RC_Channel *channel_pitch; 247 | RC_Channel *channel_throttle; 248 | RC_Channel *channel_yaw; 249 | 250 | AP_Logger logger; 251 | 252 | // flight modes convenience array 253 | AP_Int8 *flight_modes; 254 | const uint8_t num_flight_modes = 6; 255 | 256 | struct RangeFinderState { 257 | bool enabled:1; 258 | bool alt_healthy:1; // true if we can trust the altitude from the rangefinder 259 | int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder 260 | float inertial_alt_cm; // inertial alt at time of last rangefinder sample 261 | uint32_t last_healthy_ms; 262 | LowPassFilterFloat alt_cm_filt; // altitude filter 263 | int16_t alt_cm_glitch_protected; // last glitch protected altitude 264 | int8_t glitch_count; // non-zero number indicates rangefinder is glitching 265 | uint32_t glitch_cleared_ms; // system time glitch cleared 266 | float terrain_offset_cm; // filtered terrain offset (e.g. terrain's height above EKF origin) 267 | } rangefinder_state, rangefinder_up_state; 268 | 269 | // return rangefinder height interpolated using inertial altitude 270 | bool get_rangefinder_height_interpolated_cm(int32_t& ret) const; 271 | 272 | class SurfaceTracking { 273 | public: 274 | 275 | // update_surface_offset - manages the vertical offset of the position controller to follow the 276 | // measured ground or ceiling level measured using the range finder. 277 | void update_surface_offset(); 278 | 279 | // get/set target altitude (in cm) above ground 280 | bool get_target_alt_cm(float &target_alt_cm) const; 281 | void set_target_alt_cm(float target_alt_cm); 282 | 283 | // get target and actual distances (in m) for logging purposes 284 | bool get_target_dist_for_logging(float &target_dist) const; 285 | float get_dist_for_logging() const; 286 | void invalidate_for_logging() { valid_for_logging = false; } 287 | 288 | // surface tracking surface 289 | enum class Surface { 290 | NONE = 0, 291 | GROUND = 1, 292 | CEILING = 2 293 | }; 294 | // set surface to track 295 | void set_surface(Surface new_surface); 296 | // initialise surface tracking 297 | void init(Surface surf) { surface = surf; } 298 | 299 | private: 300 | Surface surface; 301 | uint32_t last_update_ms; // system time of last update to target_alt_cm 302 | uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery 303 | bool valid_for_logging; // true if we have a desired target altitude 304 | bool reset_target; // true if target should be reset because of change in surface being tracked 305 | } surface_tracking; 306 | 307 | #if AP_RPM_ENABLED 308 | AP_RPM rpm_sensor; 309 | #endif 310 | 311 | // Inertial Navigation EKF - different viewpoint 312 | AP_AHRS_View *ahrs_view; 313 | 314 | // Arming/Disarming management class 315 | AP_Arming_Copter arming; 316 | 317 | // Optical flow sensor 318 | #if AP_OPTICALFLOW_ENABLED 319 | AP_OpticalFlow optflow; 320 | #endif 321 | 322 | // system time in milliseconds of last recorded yaw reset from ekf 323 | uint32_t ekfYawReset_ms; 324 | int8_t ekf_primary_core; 325 | 326 | // vibration check 327 | struct { 328 | bool high_vibes; // true while high vibration are detected 329 | uint32_t start_ms; // system time high vibration were last detected 330 | uint32_t clear_ms; // system time high vibrations stopped 331 | } vibration_check; 332 | 333 | // takeoff check 334 | uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure 335 | 336 | // GCS selection 337 | GCS_Copter _gcs; // avoid using this; use gcs() 338 | GCS_Copter &gcs() { return _gcs; } 339 | 340 | // User variables 341 | #ifdef USERHOOK_VARIABLES 342 | # include USERHOOK_VARIABLES 343 | #endif 344 | 345 | // Documentation of GLobals: 346 | typedef union { 347 | struct { 348 | uint8_t unused1 : 1; // 0 349 | uint8_t unused_was_simple_mode : 2; // 1,2 350 | uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully 351 | uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed 352 | uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised 353 | uint8_t logging_started : 1; // 6 // true if logging has started 354 | uint8_t land_complete : 1; // 7 // true if we have detected a landing 355 | uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio 356 | uint8_t usb_connected_unused : 1; // 9 // UNUSED 357 | uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update 358 | uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration 359 | uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test 360 | uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes 361 | uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) 362 | uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock 363 | uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS 364 | uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy 365 | uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use 366 | uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position 367 | uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable 368 | uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors 369 | uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done 370 | uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set 371 | uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed 372 | uint8_t armed_with_airmode_switch : 1; // 27 // we armed using a arming switch 373 | uint8_t prec_land_active : 1; // 28 // true if precland is active 374 | }; 375 | uint32_t value; 376 | } ap_t; 377 | 378 | ap_t ap; 379 | 380 | AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled; 381 | bool force_flying; // force flying is enabled when true; 382 | 383 | static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t"); 384 | 385 | // This is the state of the flight control system 386 | // There are multiple states defined such as STABILIZE, ACRO, 387 | Mode *flightmode; 388 | Mode::Number prev_control_mode; 389 | 390 | RCMapper rcmap; 391 | 392 | // inertial nav alt when we armed 393 | float arming_altitude_m; 394 | 395 | // Failsafe 396 | struct { 397 | uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure 398 | uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed 399 | 400 | int8_t radio_counter; // number of iterations with throttle below throttle_fs_value 401 | 402 | uint8_t radio : 1; // A status flag for the radio failsafe 403 | uint8_t gcs : 1; // A status flag for the ground station failsafe 404 | uint8_t ekf : 1; // true if ekf failsafe has occurred 405 | uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred 406 | uint8_t adsb : 1; // true if an adsb related failsafe has occurred 407 | uint8_t deadreckon : 1; // true if a dead reckoning failsafe has triggered 408 | } failsafe; 409 | 410 | bool any_failsafe_triggered() const { 411 | return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb || failsafe.deadreckon; 412 | } 413 | 414 | // dead reckoning state 415 | struct { 416 | bool active; // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source) 417 | bool timeout; // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted 418 | uint32_t start_ms; // system time that EKF began deadreckoning 419 | } dead_reckoning; 420 | 421 | // Motor Output 422 | MOTOR_CLASS *motors; 423 | const struct AP_Param::GroupInfo *motors_var_info; 424 | 425 | int32_t _home_bearing; 426 | uint32_t _home_distance; 427 | 428 | // SIMPLE Mode 429 | // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming 430 | // or in SuperSimple mode when the vehicle leaves a 20m radius from home. 431 | enum class SimpleMode { 432 | NONE = 0, 433 | SIMPLE = 1, 434 | SUPERSIMPLE = 2, 435 | } simple_mode; 436 | 437 | float simple_cos_yaw; 438 | float simple_sin_yaw; 439 | int32_t super_simple_last_bearing; 440 | float super_simple_cos_yaw; 441 | float super_simple_sin_yaw; 442 | 443 | // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable 444 | int32_t initial_armed_bearing; 445 | 446 | // Battery Sensors 447 | AP_BattMonitor battery{MASK_LOG_CURRENT, 448 | FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t), 449 | _failsafe_priorities}; 450 | 451 | #if OSD_ENABLED || OSD_PARAM_ENABLED 452 | AP_OSD osd; 453 | #endif 454 | 455 | // Altitude 456 | int32_t baro_alt; // barometer altitude in cm above home 457 | LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests 458 | 459 | // filtered pilot's throttle input used to cancel landing if throttle held high 460 | LowPassFilterFloat rc_throttle_control_in_filter; 461 | 462 | // 3D Location vectors 463 | // Current location of the vehicle (altitude is relative to home) 464 | Location current_loc; 465 | 466 | // Inertial Navigation 467 | AP_InertialNav inertial_nav; 468 | 469 | // Attitude, Position and Waypoint navigation objects 470 | // To-Do: move inertial nav up or other navigation variables down here 471 | AC_AttitudeControl_t *attitude_control; 472 | AC_PosControl *pos_control; 473 | AC_WPNav *wp_nav; 474 | AC_Loiter *loiter_nav; 475 | 476 | #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED 477 | AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()}; 478 | #endif 479 | 480 | #if MODE_CIRCLE_ENABLED == ENABLED 481 | AC_Circle *circle_nav; 482 | #endif 483 | 484 | // System Timers 485 | // -------------- 486 | // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. 487 | uint32_t arm_time_ms; 488 | 489 | // Used to exit the roll and pitch auto trim function 490 | uint8_t auto_trim_counter; 491 | bool auto_trim_started = false; 492 | 493 | // Camera 494 | #if AP_CAMERA_ENABLED 495 | AP_Camera camera{MASK_LOG_CAMERA}; 496 | #endif 497 | 498 | // Camera/Antenna mount tracking and stabilisation stuff 499 | #if HAL_MOUNT_ENABLED 500 | AP_Mount camera_mount; 501 | #endif 502 | 503 | #if AC_AVOID_ENABLED == ENABLED 504 | AC_Avoid avoid; 505 | #endif 506 | 507 | // Rally library 508 | #if HAL_RALLY_ENABLED 509 | AP_Rally_Copter rally; 510 | #endif 511 | 512 | // Crop Sprayer 513 | #if HAL_SPRAYER_ENABLED 514 | AC_Sprayer sprayer; 515 | #endif 516 | 517 | // Parachute release 518 | #if PARACHUTE == ENABLED 519 | AP_Parachute parachute; 520 | #endif 521 | 522 | // Landing Gear Controller 523 | #if AP_LANDINGGEAR_ENABLED 524 | AP_LandingGear landinggear; 525 | #endif 526 | 527 | // terrain handling 528 | #if AP_TERRAIN_AVAILABLE 529 | AP_Terrain terrain; 530 | #endif 531 | 532 | // Precision Landing 533 | #if PRECISION_LANDING == ENABLED 534 | AC_PrecLand precland; 535 | AC_PrecLand_StateMachine precland_statemachine; 536 | #endif 537 | 538 | // Pilot Input Management Library 539 | // Only used for Helicopter for now 540 | #if FRAME_CONFIG == HELI_FRAME 541 | AC_InputManager_Heli input_manager; 542 | #endif 543 | 544 | #if HAL_ADSB_ENABLED 545 | AP_ADSB adsb; 546 | 547 | // avoidance of adsb enabled vehicles (normally manned vehicles) 548 | AP_Avoidance_Copter avoidance_adsb{adsb}; 549 | #endif 550 | 551 | // last valid RC input time 552 | uint32_t last_radio_update_ms; 553 | 554 | // last esc calibration notification update 555 | uint32_t esc_calibration_notify_update_ms; 556 | 557 | // Top-level logic 558 | // setup the var_info table 559 | AP_Param param_loader; 560 | 561 | #if FRAME_CONFIG == HELI_FRAME 562 | // Mode filter to reject RC Input glitches. Filter size is 5, and it draws the 4th element, so it can reject 3 low glitches, 563 | // and 1 high glitch. This is because any "off" glitches can be highly problematic for a helicopter running an ESC 564 | // governor. Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart. 565 | ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4}; 566 | 567 | // Tradheli flags 568 | typedef struct { 569 | uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) 570 | uint8_t inverted_flight : 1; // 1 // true for inverted flight mode 571 | uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation 572 | bool coll_stk_low ; // 3 // true when collective stick is on lower limit 573 | } heli_flags_t; 574 | heli_flags_t heli_flags; 575 | 576 | int16_t hover_roll_trim_scalar_slew; 577 | #endif 578 | 579 | // ground effect detector 580 | struct { 581 | bool takeoff_expected; 582 | bool touchdown_expected; 583 | uint32_t takeoff_time_ms; 584 | float takeoff_alt_cm; 585 | } gndeffect_state; 586 | 587 | bool standby_active; 588 | 589 | static const AP_Scheduler::Task scheduler_tasks[]; 590 | static const AP_Param::Info var_info[]; 591 | static const struct LogStructure log_structure[]; 592 | 593 | // enum for ESC CALIBRATION 594 | enum ESCCalibrationModes : uint8_t { 595 | ESCCAL_NONE = 0, 596 | ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1, 597 | ESCCAL_PASSTHROUGH_ALWAYS = 2, 598 | ESCCAL_AUTO = 3, 599 | ESCCAL_DISABLED = 9, 600 | }; 601 | 602 | enum class FailsafeAction : uint8_t { 603 | NONE = 0, 604 | LAND = 1, 605 | RTL = 2, 606 | SMARTRTL = 3, 607 | SMARTRTL_LAND = 4, 608 | TERMINATE = 5, 609 | AUTO_DO_LAND_START = 6, 610 | BRAKE_LAND = 7 611 | }; 612 | 613 | enum class FailsafeOption { 614 | RC_CONTINUE_IF_AUTO = (1<<0), // 1 615 | GCS_CONTINUE_IF_AUTO = (1<<1), // 2 616 | RC_CONTINUE_IF_GUIDED = (1<<2), // 4 617 | CONTINUE_IF_LANDING = (1<<3), // 8 618 | GCS_CONTINUE_IF_PILOT_CONTROL = (1<<4), // 16 619 | RELEASE_GRIPPER = (1<<5), // 32 620 | }; 621 | 622 | 623 | enum class FlightOptions { 624 | DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1 625 | DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2 626 | RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4 627 | }; 628 | 629 | static constexpr int8_t _failsafe_priorities[] = { 630 | (int8_t)FailsafeAction::TERMINATE, 631 | (int8_t)FailsafeAction::LAND, 632 | (int8_t)FailsafeAction::RTL, 633 | (int8_t)FailsafeAction::SMARTRTL_LAND, 634 | (int8_t)FailsafeAction::SMARTRTL, 635 | (int8_t)FailsafeAction::NONE, 636 | -1 // the priority list must end with a sentinel of -1 637 | }; 638 | 639 | #define FAILSAFE_LAND_PRIORITY 1 640 | static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == (int8_t)FailsafeAction::LAND, 641 | "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities"); 642 | static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, 643 | "_failsafe_priorities is missing the sentinel"); 644 | 645 | 646 | 647 | // AP_State.cpp 648 | void set_auto_armed(bool b); 649 | void set_simple_mode(SimpleMode b); 650 | void set_failsafe_radio(bool b); 651 | void set_failsafe_gcs(bool b); 652 | void update_using_interlock(); 653 | 654 | // Copter.cpp 655 | void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, 656 | uint8_t &task_count, 657 | uint32_t &log_bit) override; 658 | #if AP_SCRIPTING_ENABLED 659 | #if MODE_GUIDED_ENABLED == ENABLED 660 | bool start_takeoff(float alt) override; 661 | bool set_target_location(const Location& target_loc) override; 662 | bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override; 663 | bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override; 664 | bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override; 665 | bool set_target_velocity_NED(const Vector3f& vel_ned) override; 666 | bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override; 667 | bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; 668 | #endif 669 | #if MODE_CIRCLE_ENABLED == ENABLED 670 | bool get_circle_radius(float &radius_m) override; 671 | bool set_circle_rate(float rate_dps) override; 672 | #endif 673 | bool set_desired_speed(float speed) override; 674 | #if MODE_AUTO_ENABLED == ENABLED 675 | bool nav_scripting_enable(uint8_t mode) override; 676 | bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; 677 | void nav_script_time_done(uint16_t id) override; 678 | #endif 679 | // lua scripts use this to retrieve EKF failsafe state 680 | // returns true if the EKF failsafe has triggered 681 | bool has_ekf_failsafed() const override; 682 | #endif // AP_SCRIPTING_ENABLED 683 | void rc_loop(); 684 | void throttle_loop(); 685 | void update_batt_compass(void); 686 | void loop_rate_logging(); 687 | void ten_hz_logging_loop(); 688 | void twentyfive_hz_logging(); 689 | void three_hz_loop(); 690 | void one_hz_loop(); 691 | void init_simple_bearing(); 692 | void update_simple_mode(void); 693 | void update_super_simple_bearing(bool force_update); 694 | void read_AHRS(void); 695 | void update_altitude(); 696 | bool get_wp_distance_m(float &distance) const override; 697 | bool get_wp_bearing_deg(float &bearing) const override; 698 | bool get_wp_crosstrack_error_m(float &xtrack_error) const override; 699 | bool get_rate_ef_targets(Vector3f& rate_ef_targets) const override; 700 | 701 | // Attitude.cpp 702 | void update_throttle_hover(); 703 | float get_pilot_desired_climb_rate(float throttle_control); 704 | float get_non_takeoff_throttle(); 705 | void set_accel_throttle_I_from_pilot_throttle(); 706 | void rotate_body_frame_to_NE(float &x, float &y); 707 | uint16_t get_pilot_speed_dn() const; 708 | void run_rate_controller(); 709 | 710 | #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED 711 | void run_custom_controller() { custom_control.update(); } 712 | #endif 713 | 714 | // avoidance.cpp 715 | void low_alt_avoidance(); 716 | 717 | #if HAL_ADSB_ENABLED 718 | // avoidance_adsb.cpp 719 | void avoidance_adsb_update(void); 720 | #endif 721 | 722 | // baro_ground_effect.cpp 723 | void update_ground_effect_detector(void); 724 | void update_ekf_terrain_height_stable(); 725 | 726 | // commands.cpp 727 | void update_home_from_EKF(); 728 | void set_home_to_current_location_inflight(); 729 | bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; 730 | bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; 731 | bool far_from_EKF_origin(const Location& loc); 732 | 733 | // compassmot.cpp 734 | MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan); 735 | 736 | // crash_check.cpp 737 | void crash_check(); 738 | void thrust_loss_check(); 739 | void yaw_imbalance_check(); 740 | LowPassFilterFloat yaw_I_filt{0.05f}; 741 | uint32_t last_yaw_warn_ms; 742 | void parachute_check(); 743 | void parachute_release(); 744 | void parachute_manual_release(); 745 | 746 | // ekf_check.cpp 747 | void ekf_check(); 748 | bool ekf_over_threshold(); 749 | void failsafe_ekf_event(); 750 | void failsafe_ekf_off_event(void); 751 | void failsafe_ekf_recheck(); 752 | void check_ekf_reset(); 753 | void check_vibration(); 754 | 755 | // esc_calibration.cpp 756 | void esc_calibration_startup_check(); 757 | void esc_calibration_passthrough(); 758 | void esc_calibration_auto(); 759 | void esc_calibration_notify(); 760 | void esc_calibration_setup(); 761 | 762 | // events.cpp 763 | bool failsafe_option(FailsafeOption opt) const; 764 | void failsafe_radio_on_event(); 765 | void failsafe_radio_off_event(); 766 | void handle_battery_failsafe(const char* type_str, const int8_t action); 767 | void failsafe_gcs_check(); 768 | void failsafe_gcs_on_event(void); 769 | void failsafe_gcs_off_event(void); 770 | void failsafe_terrain_check(); 771 | void failsafe_terrain_set_status(bool data_ok); 772 | void failsafe_terrain_on_event(); 773 | void gpsglitch_check(); 774 | void failsafe_deadreckon_check(); 775 | void set_mode_RTL_or_land_with_pause(ModeReason reason); 776 | void set_mode_SmartRTL_or_RTL(ModeReason reason); 777 | void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); 778 | void set_mode_auto_do_land_start_or_RTL(ModeReason reason); 779 | void set_mode_brake_or_land_with_pause(ModeReason reason); 780 | bool should_disarm_on_failsafe(); 781 | void do_failsafe_action(FailsafeAction action, ModeReason reason); 782 | void announce_failsafe(const char *type, const char *action_undertaken=nullptr); 783 | 784 | // failsafe.cpp 785 | void failsafe_enable(); 786 | void failsafe_disable(); 787 | #if ADVANCED_FAILSAFE == ENABLED 788 | void afs_fs_check(void); 789 | #endif 790 | 791 | // fence.cpp 792 | #if AP_FENCE_ENABLED 793 | void fence_check(); 794 | #endif 795 | 796 | // heli.cpp 797 | void heli_init(); 798 | void check_dynamic_flight(void); 799 | bool should_use_landing_swash() const; 800 | void update_heli_control_dynamics(void); 801 | void heli_update_landing_swash(); 802 | float get_pilot_desired_rotor_speed() const; 803 | void heli_update_rotor_speed_targets(); 804 | void heli_update_autorotation(); 805 | void update_collective_low_flag(int16_t throttle_control); 806 | 807 | // inertia.cpp 808 | void read_inertia(); 809 | 810 | // landing_detector.cpp 811 | void update_land_and_crash_detectors(); 812 | void update_land_detector(); 813 | void set_land_complete(bool b); 814 | void set_land_complete_maybe(bool b); 815 | void update_throttle_mix(); 816 | 817 | #if AP_LANDINGGEAR_ENABLED 818 | // landing_gear.cpp 819 | void landinggear_update(); 820 | #endif 821 | 822 | // standby.cpp 823 | void standby_update(); 824 | 825 | // Log.cpp 826 | void Log_Write_Control_Tuning(); 827 | void Log_Write_Attitude(); 828 | void Log_Write_EKF_POS(); 829 | void Log_Write_PIDS(); 830 | void Log_Write_Data(LogDataID id, int32_t value); 831 | void Log_Write_Data(LogDataID id, uint32_t value); 832 | void Log_Write_Data(LogDataID id, int16_t value); 833 | void Log_Write_Data(LogDataID id, uint16_t value); 834 | void Log_Write_Data(LogDataID id, float value); 835 | void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); 836 | void Log_Video_Stabilisation(); 837 | #if FRAME_CONFIG == HELI_FRAME 838 | void Log_Write_Heli(void); 839 | #endif 840 | void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target); 841 | void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate); 842 | void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); 843 | void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); 844 | void Log_Write_Vehicle_Startup_Messages(); 845 | void log_init(void); 846 | 847 | // mode.cpp 848 | bool set_mode(Mode::Number mode, ModeReason reason); 849 | bool set_mode(const uint8_t new_mode, const ModeReason reason) override; 850 | ModeReason _last_reason; 851 | // called when an attempt to change into a mode is unsuccessful: 852 | void mode_change_failed(const Mode *mode, const char *reason); 853 | uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); } 854 | void update_flight_mode(); 855 | void notify_flight_mode(); 856 | 857 | // mode_land.cpp 858 | void set_mode_land_with_pause(ModeReason reason); 859 | bool landing_with_GPS(); 860 | 861 | // motor_test.cpp 862 | void motor_test_output(); 863 | bool mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode); 864 | MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count); 865 | void motor_test_stop(); 866 | 867 | // motors.cpp 868 | void arm_motors_check(); 869 | void auto_disarm_check(); 870 | void motors_output(); 871 | void lost_vehicle_check(); 872 | 873 | // navigation.cpp 874 | void run_nav_updates(void); 875 | int32_t home_bearing(); 876 | uint32_t home_distance(); 877 | 878 | // Parameters.cpp 879 | void load_parameters(void) override; 880 | void convert_pid_parameters(void); 881 | #if HAL_PROXIMITY_ENABLED 882 | void convert_prx_parameters(); 883 | #endif 884 | void convert_lgr_parameters(void); 885 | void convert_tradheli_parameters(void) const; 886 | 887 | // precision_landing.cpp 888 | void init_precland(); 889 | void update_precland(); 890 | 891 | // radio.cpp 892 | void default_dead_zones(); 893 | void init_rc_in(); 894 | void init_rc_out(); 895 | void enable_motor_output(); 896 | void read_radio(); 897 | void set_throttle_and_failsafe(uint16_t throttle_pwm); 898 | void set_throttle_zero_flag(int16_t throttle_control); 899 | void radio_passthrough_to_motors(); 900 | int16_t get_throttle_mid(void); 901 | 902 | // sensors.cpp 903 | void read_barometer(void); 904 | void init_rangefinder(void); 905 | void read_rangefinder(void); 906 | bool rangefinder_alt_ok() const; 907 | bool rangefinder_up_ok() const; 908 | void update_rangefinder_terrain_offset(); 909 | void update_optical_flow(void); 910 | 911 | // takeoff_check.cpp 912 | void takeoff_check(); 913 | 914 | // RC_Channel.cpp 915 | void save_trim(); 916 | void auto_trim(); 917 | void auto_trim_cancel(); 918 | 919 | // system.cpp 920 | void init_ardupilot() override; 921 | void startup_INS_ground(); 922 | bool position_ok() const; 923 | bool ekf_has_absolute_position() const; 924 | bool ekf_has_relative_position() const; 925 | bool ekf_alt_ok() const; 926 | void update_auto_armed(); 927 | bool should_log(uint32_t mask); 928 | const char* get_frame_string() const; 929 | void allocate_motors(void); 930 | bool is_tradheli() const; 931 | 932 | // terrain.cpp 933 | void terrain_update(); 934 | void terrain_logging(); 935 | 936 | // tuning.cpp 937 | void tuning(); 938 | 939 | // UserCode.cpp 940 | void userhook_init(); 941 | void userhook_FastLoop(); 942 | void userhook_50Hz(); 943 | void userhook_MediumLoop(); 944 | void userhook_SlowLoop(); 945 | void userhook_SuperSlowLoop(); 946 | void userhook_auxSwitch1(const RC_Channel::AuxSwitchPos ch_flag); 947 | void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag); 948 | void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag); 949 | 950 | #if MODE_ACRO_ENABLED == ENABLED 951 | #if FRAME_CONFIG == HELI_FRAME 952 | ModeAcro_Heli mode_acro; 953 | #else 954 | ModeAcro mode_acro; 955 | #endif 956 | #endif 957 | ModeAltHold mode_althold; 958 | #if MODE_AUTO_ENABLED == ENABLED 959 | ModeAuto mode_auto; 960 | #endif 961 | #if AUTOTUNE_ENABLED == ENABLED 962 | ModeAutoTune mode_autotune; 963 | #endif 964 | #if MODE_BRAKE_ENABLED == ENABLED 965 | ModeBrake mode_brake; 966 | #endif 967 | #if MODE_CIRCLE_ENABLED == ENABLED 968 | ModeCircle mode_circle; 969 | #endif 970 | #if MODE_DRIFT_ENABLED == ENABLED 971 | ModeDrift mode_drift; 972 | #endif 973 | #if MODE_FLIP_ENABLED == ENABLED 974 | ModeFlip mode_flip; 975 | #endif 976 | #if MODE_FOLLOW_ENABLED == ENABLED 977 | ModeFollow mode_follow; 978 | #endif 979 | #if MODE_GUIDED_ENABLED == ENABLED 980 | ModeGuided mode_guided; 981 | #endif 982 | ModeLand mode_land; 983 | #if MODE_LOITER_ENABLED == ENABLED 984 | ModeLoiter mode_loiter; 985 | #endif 986 | #if MODE_POSHOLD_ENABLED == ENABLED 987 | ModePosHold mode_poshold; 988 | #endif 989 | #if MODE_RTL_ENABLED == ENABLED 990 | ModeRTL mode_rtl; 991 | #endif 992 | #if FRAME_CONFIG == HELI_FRAME 993 | ModeStabilize_Heli mode_stabilize; 994 | #else 995 | ModeStabilize mode_stabilize; 996 | #endif 997 | #if MODE_SPORT_ENABLED == ENABLED 998 | ModeSport mode_sport; 999 | #endif 1000 | #if MODE_SYSTEMID_ENABLED == ENABLED 1001 | ModeSystemId mode_systemid; 1002 | #endif 1003 | #if HAL_ADSB_ENABLED 1004 | ModeAvoidADSB mode_avoid_adsb; 1005 | #endif 1006 | #if MODE_THROW_ENABLED == ENABLED 1007 | ModeThrow mode_throw; 1008 | #endif 1009 | #if MODE_GUIDED_NOGPS_ENABLED == ENABLED 1010 | ModeGuidedNoGPS mode_guided_nogps; 1011 | #endif 1012 | #if MODE_SMARTRTL_ENABLED == ENABLED 1013 | ModeSmartRTL mode_smartrtl; 1014 | #endif 1015 | #if MODE_FLOWHOLD_ENABLED == ENABLED 1016 | ModeFlowHold mode_flowhold; 1017 | #endif 1018 | #if MODE_ZIGZAG_ENABLED == ENABLED 1019 | ModeZigZag mode_zigzag; 1020 | #endif 1021 | #if MODE_AUTOROTATE_ENABLED == ENABLED 1022 | ModeAutorotate mode_autorotate; 1023 | #endif 1024 | #if MODE_TURTLE_ENABLED == ENABLED 1025 | ModeTurtle mode_turtle; 1026 | #endif 1027 | #if MODE_ADAPTIVE_ENABLED == ENABLED 1028 | ModeAdaptive mode_adaptive; 1029 | #endif 1030 | 1031 | // mode.cpp 1032 | Mode *mode_from_mode_num(const Mode::Number mode); 1033 | void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); 1034 | 1035 | public: 1036 | void failsafe_check(); // failsafe.cpp 1037 | }; 1038 | 1039 | extern Copter copter; 1040 | 1041 | using AP_HAL::millis; 1042 | using AP_HAL::micros; 1043 | --------------------------------------------------------------------------------