Skip to content
Snippets Groups Projects
Commit 60bc9f45 authored by Randy Mackay's avatar Randy Mackay
Browse files

TradHeli: angle error to zero while motors runup

Set angle error to zero in get_roll_rate_stabilized_bf,
get_pitch_rate_stabillize_bf, get_yaw_rate_stabilized_bf.
Original commit by Rob Lefebvre
parent c9c803ff
No related branches found
No related tags found
No related merge requests found
......@@ -187,9 +187,15 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
// don't let angle error grow too large
angle_error = constrain_float(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete()) {
angle_error = 0;
}
#else
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
}
// Pitch with rate input and stabilized in the body frame
......@@ -226,9 +232,15 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
// don't let angle error grow too large
angle_error = constrain_float(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete()) {
angle_error = 0;
}
#else
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
}
// Yaw with rate input and stabilized in the body frame
......@@ -265,9 +277,15 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
// don't let angle error grow too large
angle_error = constrain_float(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete()) {
angle_error = 0;
}
#else
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
}
// Roll with rate input and stabilized in the earth frame
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment