Browse Source

rtl 几个if语句调整

mission-4.1.18
zbr 3 years ago
parent
commit
64f699d66d
  1. 15
      ArduCopter/mode_rtl.cpp
  2. 2
      modules/mavlink

15
ArduCopter/mode_rtl.cpp

@ -202,8 +202,9 @@ void ModeRTL::climb_return_run() @@ -202,8 +202,9 @@ void ModeRTL::climb_return_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
if(g.zr_use_rc)
if(g.zr_use_rc){
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -264,8 +265,9 @@ void ModeRTL::loiterathome_run() @@ -264,8 +265,9 @@ void ModeRTL::loiterathome_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
if(g.zr_use_rc)
if(g.zr_use_rc){
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -350,11 +352,11 @@ void ModeRTL::descent_run() @@ -350,11 +352,11 @@ void ModeRTL::descent_run()
update_simple_mode();
// convert pilot input to lean angles
if(g.zr_use_rc)
if(g.zr_use_rc){
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
}
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
if(!is_zero(target_roll) || !is_zero(target_pitch)){
if (!copter.ap.land_repo_active) {
copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE);
}
@ -363,9 +365,10 @@ void ModeRTL::descent_run() @@ -363,9 +365,10 @@ void ModeRTL::descent_run()
}
// get pilot's desired yaw rate
if(g.zr_use_rc)
if(g.zr_use_rc){
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 72b407db3991ed74631fa59a5e826615f569819f
Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80
Loading…
Cancel
Save