Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

Plane: tiltrotors: allow vectored yaw motor tilt when disarmed #12756

Merged
merged 2 commits into from
Sep 8, 2020

Conversation

kd0aij
Copy link
Contributor

@kd0aij kd0aij commented Nov 3, 2019

Useful for checking the vectored yaw tilt direction and range when setting up tilt SERVO MAX/MIN and Q_TILT_YAW_ANGLE

@IamPete1
Copy link
Member

IamPete1 commented Nov 3, 2019

I'm don't think this is more intuitive than yawing the plane and seeing the yaw tilt motor move to counteract. That is what I think it does now?

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 3, 2019

It does nothing when disarmed, and I've never seen the tilt servos react to yaw without the props spinning.
This also lets you check for binding at the extremes of yaw tilt.

@IamPete1
Copy link
Member

IamPete1 commented Nov 3, 2019

Its been a long time since I have played with VTOL stuff IRL. I thinking again your correct, in that case I think this makes sense. The only possible issue is that in arming it will do a full yaw. On some vehicles this might make the props hit the ground. Is there a options bitmask parameter somewhere we could put this in?

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 3, 2019

Yes, this has the potential disadvantage that rudder arm/disarm results in maximum yaw tilt.
But vectored yaw uses a relatively small amount of tilt on normal tiltrotors,

And when you put a Convergence or a TVBS into FBWA the motors promptly tilt all the way forward, even when disarmed, which pretty much guarantees that props will scrape the ground.
So we have a precedent :)

I don't think there are any existing params which could be exploited to turn this on and off.

@tridge
Copy link
Contributor

tridge commented Nov 4, 2019

this could spin up while tilted

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 4, 2019

Demo with Q_M_SPIN_ARM and Q_M_SPIN_MIN set to 0.15. Note that on arming with rudder, motors return to vertical during the warning beep and before spooling up. But on rudder disarm, the motors tilt while coasting down.
https://photos.app.goo.gl/NJYhPQwg4jmgnv6w5

added 3 second delay from disarm to tilt

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 25, 2019

Copter's ARMING_DELAY is not implemented in quadplane. Commit 51e204c adds a const expression: ARMING_DELAY_MS = 2000; and delays spoolup in motors_output for that length of time. This also allows time for the motor tilt to return to vertical on rudder arming.

@IamPete1
Copy link
Member

You could put the this and the delay as a bit on this param

AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),

So by default there is no change, if you enable the bit you get the tilting and the delay

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 25, 2019

Cool... Question is whether this is a safety issue that was overlooked in quadplane. In Copter (and PX4) there is an intentional delay between arming (and the start of the arming tone) and the motors spinning up. Gives you time to move your fingers out of the way :)
Probably should be a separate PR...

@IamPete1
Copy link
Member

you could always give them a bit each ;)

@kd0aij
Copy link
Contributor Author

kd0aij commented Nov 26, 2019

put disarmed tilt on a q_option bit and only do the delays if it is enabled
The spool-up delay is always applied if OPTION_DISARMED_TILT is set, to allow motors to return to vertical before spinning.
If OPTION_DELAY_ARMING is set, the spool-up delay is always applied.

@kd0aij kd0aij force-pushed the pr-tilt-disarmed branch 2 times, most recently from c9d5f28 to 9ea8f50 Compare December 4, 2019 22:24
@kd0aij kd0aij closed this Dec 16, 2019
@kd0aij kd0aij reopened this Dec 16, 2019
@kd0aij
Copy link
Contributor Author

kd0aij commented Dec 19, 2019

@tridge I think this is acceptable now...

@kd0aij
Copy link
Contributor Author

kd0aij commented Jun 5, 2020

rebased and tested on miniConvergence with mRoX21

@kd0aij
Copy link
Contributor Author

kd0aij commented Jun 9, 2020

@tridge added the missing parentheses and squashed; ready to merge if it passes CI

ArduPlane/quadplane.cpp Outdated Show resolved Hide resolved
ArduPlane/quadplane.cpp Show resolved Hide resolved
motors->output();
return;
} else {
hal.util->reset_delay_arming();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this belongs at the hal level, it creates a dependency on ARMING_DELAY_MS in the hal

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, won't consider consolidating the arming delay with copter

@kd0aij
Copy link
Contributor Author

kd0aij commented Jun 16, 2020

@tridge moved arming_delay from HAL to quadplane

@kd0aij kd0aij requested a review from tridge June 16, 2020 03:55
@kd0aij
Copy link
Contributor Author

kd0aij commented Jun 16, 2020

moving arming_delay logic to AP_Arming_Plane gives better localization and simplifies the logic in quadplane.cpp

SITL test:

QSTABILIZE> arm throttle
10859: set delay_arming
APM: Throttle armed
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
10866: delaying spoolup
ARMED
Arming checks disabled
11866: delaying spoolup
12859: reset delay_arming
APM: EKF yaw reset -0.44
APM: EKF2 IMU0 origin set
APM: EKF2 IMU1 origin set
QSTABILIZE> disarm
APM: Throttle disarmed
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
27264: delaying tilt
DISARMED
28264: delaying tilt
29264: delaying tilt

@kd0aij
Copy link
Contributor Author

kd0aij commented Sep 5, 2020

demonstration of disarmed motor tilt and rudder arm/disarm with spoolup delayed till end of alarm tone
https://youtu.be/2q72UJIULIg

add disarm tilt delay
add arming delay
add Q_OPTIONS for disarmed motor tilt and delayed arming
add comment explaining arming delay option
eliminate millis() wrap in arming delay
@tridge tridge merged commit 629f215 into ArduPilot:master Sep 8, 2020
@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Sep 8, 2020
@Hwurzburg Hwurzburg removed the WikiNeeded needs wiki update label Sep 23, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

5 participants