Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Clarify calculation used to detect throw

This commit is contained in:
Paul Riseborough 2021-01-12 16:04:35 +11:00 committed by Andrew Tridgell
parent 3634a942a1
commit 1dc79f0be8
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/servos.cpp
View File

@ -557,7 +557,12 @@ void Plane::set_servos_controlled(void)
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (arming.is_armed()) {
bool throw_detected = ahrs.get_accel().x - GRAVITY_MSS * ahrs.sin_pitch() > GRAVITY_MSS;
// Check if rate of change of velocity along X axis exceeds 1-g which normally indicates a throw.
// Tests with hand carriage of micro UAS indicates that a 1-g threshold does not false trigger prior
// to the throw, but there is margin to increase this threshold if false triggering becomes problematic.
const float accel_x_due_to_gravity = GRAVITY_MSS * ahrs.sin_pitch();
const float accel_x_due_to_throw = ahrs.get_accel().x - accel_x_due_to_gravity;
bool throw_detected = accel_x_due_to_throw > GRAVITY_MSS;
bool throttle_up_detected = throttle > aparm.throttle_cruise;
if (throw_detected || throttle_up_detected) {
plane.ahrs.setTakeoffExpected(true);