Browse Source

plane: reinstate tailsitter roll limit

mission-4.1.18
IamPete1 6 years ago committed by Andrew Tridgell
parent
commit
02d976f264
  1. 12
      ArduPlane/mode_qstabilize.cpp

12
ArduPlane/mode_qstabilize.cpp

@ -26,12 +26,15 @@ void ModeQStabilize::update() @@ -26,12 +26,15 @@ void ModeQStabilize::update()
}
// set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit;
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit);
float pitch_input = plane.channel_pitch->norm_input();
// Scale from normalized input [-1,1] to centidegrees
if (plane.quadplane.tailsitter_active()) {
// For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX]
// separate limit for tailsitter roll, if set
if (plane.quadplane.tailsitter.max_roll_angle > 0) {
roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f;
}
// angle max for tailsitter pitch
plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
} else {
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
@ -43,5 +46,8 @@ void ModeQStabilize::update() @@ -43,5 +46,8 @@ void ModeQStabilize::update()
}
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
}
plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit;
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit);
}

Loading…
Cancel
Save