Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed WAIT_ALTITUDE wiggle

Browse Source 
when wiggle value is zero servos should be neutral
This commit is contained in:
Andrew Tridgell 2019-03-30 08:22:40 +11:00
parent b7957e820f
commit 954b6ae791
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/servos.cpp
View File

@ -251,10 +251,14 @@ void Plane::dspoiler_update(void)
*/
void Plane::set_servos_idle(void)
{
int16_t servo_value = 0;
int16_t servo_value;
// move over full range for 2 seconds
auto_state.idle_wiggle_stage += 2;
if (auto_state.idle_wiggle_stage < 50) {
if (auto_state.idle_wiggle_stage != 0) {
auto_state.idle_wiggle_stage += 2;
}
if (auto_state.idle_wiggle_stage == 0) {
servo_value = 0;
} else if (auto_state.idle_wiggle_stage < 50) {
servo_value = auto_state.idle_wiggle_stage * (4500 / 50);
} else if (auto_state.idle_wiggle_stage < 100) {
servo_value = (100 - auto_state.idle_wiggle_stage) * (4500 / 50);
@ -264,6 +268,7 @@ void Plane::set_servos_idle(void)
servo_value = (auto_state.idle_wiggle_stage-200) * (4500 / 50);
} else {
auto_state.idle_wiggle_stage = 0;
servo_value = 0;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value);