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

Sub: Do nothing for update_throttle_thr_mix

This commit is contained in:
jaxxzer 2016-02-24 18:39:26 -05:00 committed by Andrew Tridgell
parent e8b27b9d19
commit 8b337a9c7e
1 changed files with 1 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ArduSub/land_detector.cpp
View File

@ -52,42 +52,5 @@ void Sub::set_land_complete(bool b)
// has no effect when throttle is above hover throttle
void Sub::update_throttle_thr_mix()
{
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
return;
}
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
if(channel_throttle->control_in <= 0) {
motors.set_throttle_mix_min();
} else {
motors.set_throttle_mix_mid();
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
// check for large external disturbance - angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
// check for large acceleration - falling or high turbulence
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f);
// check for requested decent
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
} else {
motors.set_throttle_mix_min();
}
}
return; // placeholder, was used by heli only
}