|
|
|
@ -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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|