Drone flipped upside down when switched to manual mode
-
@Alex-Kushleyev We were doing a flight test and were experiencing some drift in GPS mode. When the pilot switched to manual mode, the drone flipped upside down and drove itself to the ground. Can you help us analyze the flight log and help us understand what happened.
I have pulled the log from
data/px4/log
-- are there any other logs that I should grab?Our setup:
- Custom drone platform
- VOXL2 as the flight controller
- FPV ESC 4-in-1
- 5G board
- Doodle Labs radio
- Jeti RC (in USB mode on the GCS over the radios)
In QGC we had performed the initial rate PID tuning, landed and fully restarted the drone. Took off again and noticed the GPS wandering. Moved higher to rule out magnetic interference and GPS shadowing, but still had drift. Switched to manual mode and it flipped and crashed.
-
@restore , it looks like your ESC tuning is off (which may or may not have caused your issue). Take a look at the rpm plot right before the crash happened:
Look how ESC2 actual rpm (blue) is much higher than ESC2 RPM CMd (cyan). For other motors, too, RPM3 (black) is much higher than ESC3 CMD (purple).
Can you provide more details:
- what motor / propeller are you using?
- have you performed the ESC calibration doc
- it looks like your motor may be low kv, have you checked and tested your ESC according to this doc
It looks like right during the switchover from gps control to manual, there was a actuator controls wiggle (see plot below). That caused ESC0 command to quickly dip and come back up. ESC0 (red) responded, rpm dipped trying to follow the command, then came back up very quickly, but then something happened (potentially de-sync) which cased the motor to coast down (between 2:50.5 and 2:52 or so and then continuing to 2:52.750 at which point the stall detection on the ESC reacted, stopped the motor and tried to restart it.
It definitely looks like the ESC did not follow the command, which could be related to tuning. If you can share the motor/prop information and the ESC params that you used i can investigate further.
-
- T-Motor Antigravity MN5006 Motor with P18*6.1 Props
- We are running a 6S Li-ion battery.
- yes we did the ESC tuning after following the low kv instructions you gave us in the ESC Power monitor thread
It has seemed like we didn't have things right - without props, the motors didn't want to spin. We are fine down to 500 RPM with props.
The min RPM in QGC was 1000 and we tuned the max to match the highest that the motors could spin at (around 10500)
Params file is below:
<?xml version="1.0" encoding="UTF-8"?> <!-- Copyright (c) 2020 ModalAI Inc. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 4. The Software is used solely in conjunction with devices provided by ModalAI Inc. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. For a license to use on non-ModalAI hardware, please contact license@modalai.com --> <!-- Sentinel v1 parameters for 3 cell battery, SunnySky 2216 880kv Motors & 10inch M-propellers --> <EscParameters> <IdParams> <param name="id" value="127"/> <!-- 0-7 .. 127 means use hardware ID pins to read ID--> <param name="dir" value="2"/> <!-- 0=fwd, 1=rev, 2=fwd id-based, 3=rev id-based --> </IdParams> <UartParams> <param name="protocol_version" value="2"/> <!-- reserved for future use --> <param name="input_mode" value="0"/> <!-- reserved for future use --> <param name="baud_rate" value="2000000"/> <!-- communication bit rate --> <param name="char_timeout_ns" value="0"/> <!-- not used --> <param name="cmd_timeout_ns" value="100000000"/> <!-- timeout for incoming commands before ESC will stop the motor --> </UartParams> <TuneParams> <param name="pwm_frequency" value="48000"/> <!-- switching freqency of PWM signal going to motors. 24Khz and 48Khz are only options for now --> <param name="vbat_nominal_mv" value="22200"/> <!-- used for sanity checking and limiting of voltage-dependent funcions --> <param name="num_cycles_per_rev" value="7"/> <!-- number of pole pairs in the motor. used for converting electrical frequency to mechanical rpm --> <param name="min_rpm" value="1500"/> <!-- minimum RPM that will be attempted, otherwise capped --> <param name="max_rpm" value="7400"/> <!-- maximum RPM that will be attempted, otherwise capped --> <param name="min_pwm" value="75"/> <!-- cap for minimum power to be ever applied. max is 999 --> <param name="max_pwm" value="999"/> <!-- cap for maximum power to be ever applied. max is 999 --> <param name="pwm_vs_rpm_curve_a0" value="823"/> <!-- this is actually motor_voltage vs rpm curve.. using legacy naming --> <param name="pwm_vs_rpm_curve_a1" value="0.5"/> <param name="pwm_vs_rpm_curve_a2" value="0.0003"/> <param name="kp" value="60"/> <!-- RPM controller proportional gain --> <param name="ki" value="20"/> <!-- RPM controller proportional gain --> <param name="max_kpe" value="300"/> <!-- maximum proportional erorr term (max is 999) --> <param name="max_kie" value="100"/> <!-- maximum integral error term (max is 999) --> <param name="max_rpm_delta" value="900"/> <!-- cap for maximum rpm error used in RPM controller --> <param name="spinup_type" value="1"/> <!-- 0: traditional, 1: sinusoidal --> <param name="spinup_power" value="150"/> <!-- power used to during spin-up procedure --> <param name="latch_power" value="120"/> <!-- power used during latching stage of spin-up (out of 999)--> <param name="spinup_power_ramp" value="8"/> <!-- it will take ( 4096 / (spinup_power_ramp*10000) ) seconds to ramp sinusoidal start-up power from 0 to spinup_power --> <param name="spinup_rpm_target" value="1000"/> <!-- Desired RPM at the end of the sinusoidal spin-up procedure --> <param name="spinup_time_ms" value="1000"/> <!-- Duration of the sinusoidal spin-up procedure --> <param name="spinup_bemf_comp" value="1"/> <!-- 0: disable, 1:enable back-emf compensation in sinusoidal spin-up procedure --> <param name="motor_kv" value="450"/> <!-- kV value of the motor. used in back-emf compensation during spin-up --> <param name="min_num_cross_for_closed_loop" value="5"/> <!-- exit latching mode of fixed power after this number of zero crossings --> <param name="protection_stall_check_rpm" value="500"/> <!-- if motor spins below this RPM, stall check will trigger and stop / restart the motor --> <param name="brake_to_stop" value="0"/> <!-- apply brake when stopping motor (or not) --> <param name="stall_timeout_ns" value="20000000"/> <!-- after spin-up, if no zero crossing is not detected for this amount of time, motor is considered stalled --> <param name="require_reset_if_stalled" value="0"/> <!-- require sending an array of zero commands to reset before next spin-up, if motor stalled --> <!--simple defualt chime--> <param name="tone_freqs" value="[200, 215, 225, 250, 0,0,0,0, 0,0,0,0]"/> <!-- 200 is 2000Hz, max 255 --> <param name="tone_durations" value="[10, 10, 10, 10, 0,0,0,0, 0,0,0,0]"/> <!-- duration of each tone in units of 10 milli-seconds. Poor naming!!! --> <param name="tone_powers" value="[60, 60, 60, 60, 0,0,0,0, 0,0,0,0]"/> <!-- max is 255 --> <param name="dt_threshold_ns" value="100000"/> <!-- during start up, ignore inter-commutation times less than this val, probably noise --> <param name="max_dt_ns" value="3500000"/> <!-- min and max values for time between two commutations. these are used as caps --> <param name="min_dt_ns" value="10000"/> <param name="dt_bootstrap_ns" value="2500000"/> <!-- filter bootstrap value for commutation dt during start up --> <param name="spinup_stall_dt_ns" value="8000000"/> <!-- during spin-up, if no zero crossing is not detected for this amount of time, motor is considered stalled --> <param name="spinup_stall_check_ns" value="30000000"/> <!-- time after beginning of spinup to start checking for spinup stall --> <param name="alignment_time_ns" value="0"/> <!-- alignment time before spin-up --> <param name="timing_advance" value="60"/> <param name="sense_advance" value="20"/> <param name="demag_timing" value="0"/> <!-- unused --> </TuneParams> </EscParameters>
-
@restore , your motor has the following winding configuration : 24N28P , which means it has 28 poles (14 pole pairs). However, in the params you have not updated the number to
14
:<param name="num_cycles_per_rev" value="7"/> <!-- number of pole pairs in the motor. used for converting electrical frequency to mechanical rpm -->
(this is mentioned in the calibration doc here
So before doing any more testing, i recommend that you:
- update
num_cycles_per_rev
from7
to14
- if you have a optical tachometer, you can actually confirm correct rpm reported by ESC (good to do once just for sanity checking)
- re-run the calibration procedure for the RPM control (and update the resulting a0, a1, a2 params)
- update min and max rpms in ESC params and PX4
- i would recommend setting in PX4 the min_rpm slightly above the absolute minimum that the ESC can achieve. You dont want the motor to spin at bare minimum rpm because it will have almost zero thrust and the attitude control can be a bit unstable when propeller is spinning so slowly (propeller will have very low "disk loading" which results in unstable thrust generation at very low rpm).
- When i say slightly above, lets say your rpm range is 1000-5000rpm, you can also use a thrust test rig to measure thrust vs rpm.. typically thrust = a *rpm^2, so between 1000rpm and 5000rpm you will have about 25x difference in thrust, so if at full thrust you generate 1000g of thrust, at 1000rpm you will only generate 40g of thrust, which is almost nothing for a large vehicle. So in this case, in flight, i would recommend capping the min rpm around 1250-1500 (in PX4 params). you can still leave the ESC params at absolute min, in this example 1000rpm.
- once again, the reason for not letting the rpms drop to absolute minimum is to avoid instabilities that can sometimes be caused by the motor spinning at very low rpm (low disk loading, also ESC response is a bit slower)
- for initial testing, you can set kp and ki to zero, for smoother response. The ESC will behave closer to traditional ESC but still have good RPM control and voltage compensation.
- (recommendation) you may be able to reduce the
spinup_power
parameter, the default is pretty high, you can see how much current one motor draws during spin-up, usually 1-2 amps is good. It will not hurt to have the higher value, it will have more torque during spinup, but will draw more current. (we can address that later)
Also, can you please confirm which firmware version you are using on your FPV ESC?
I am not 100% convinced that the misconfiguration has caused your issue, but lets get those params corrected and if possible, i'd like to see some of your tests results from the low-kv tuning. (which tests did you run and do you have plots available?)
Thanks!
- update
-
@Alex-Kushleyev Thank you for catching that.
ESC info below:
Board : version 42: ModalAi 4-in-1 ESC (M0138-1) UID : 0x203330385246571200450053 Firmware : version 39, hash 17d64675 Bootloader : version 184, hash e1c038de
-
@restore , thanks.
One more thing, when you entrer the pwm_vs_rpm_curve_a1 and a2 params, you currently have only one significant digit, which may explain why your rpm response was not working well. You should enter at least 3-4 significant digits, so instead of a2=0.003, you should put 0.003xyz where xyz are the additional digits from the original calibration results.