zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ACM: Altitude Adjust

mission-4.1.18
Jason Short 13 years ago
parent
98cc65862d
commit
9d64bea2ef
1 changed files with 21 additions and 4 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      ArduCopter/ArduCopter.pde

25
ArduCopter/ArduCopter.pde
Unescape View File

@ -1679,8 +1679,8 @@ void update_roll_pitch_mode(void) @@ -1679,8 +1679,8 @@ void update_roll_pitch_mode(void)
update_simple_mode();
}
// mix in user control with Nav control
nav_roll += constrain(wrap_180(auto_roll - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
nav_pitch += constrain(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
nav_roll += constrain(wrap_180(auto_roll - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
nav_pitch += constrain(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
@ -1699,8 +1699,8 @@ void update_roll_pitch_mode(void) @@ -1699,8 +1699,8 @@ void update_roll_pitch_mode(void)
control_pitch = g.rc_2.control_in;
// mix in user control with optical flow
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll));
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch));
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll));
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch));
break;
// THOR
@ -1834,6 +1834,7 @@ void update_throttle_mode(void) @@ -1834,6 +1834,7 @@ void update_throttle_mode(void)
case THROTTLE_HOLD:
// allow interactive changing of atitude
/*
if(g.rc_3.control_in < 200) {
reset_throttle_counter = 150;
nav_throttle = get_throttle_rate(-120);
@ -1845,6 +1846,22 @@ void update_throttle_mode(void) @@ -1845,6 +1846,22 @@ void update_throttle_mode(void)
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
}
*/
if(g.rc_3.radio_in < (g.rc_3.radio_min + 200)){
int16_t _rate = 120 - (((g.rc_3.radio_in - g.rc_3.radio_min) * 12) / 20);
reset_throttle_counter = 150;
nav_throttle = get_throttle_rate(-_rate);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
}else if(g.rc_3.radio_in > (g.rc_3.radio_max - 200)){
int16_t _rate = 180 - ((g.rc_3.radio_max - g.rc_3.radio_in) * 18) / 20;
reset_throttle_counter = 150;
nav_throttle = get_throttle_rate(_rate);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
}
// allow 1 second of slow down after pilot moves throttle back into deadzone
if(reset_throttle_counter > 0) {

Write Preview
Loading…
Cancel
Save