aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: quadplane: limit pitch for all transitions into position control modes

Browse Source
This commit is contained in:
Iampete1 2021-11-06 19:48:13 +00:00 committed by Andrew Tridgell
parent 42412b2a60
commit 9210488550
4 changed files with 99 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduPlane/mode_qloiter.cpp
View File

@ -76,18 +76,10 @@ void ModeQLoiter::run()
plane.nav_roll_cd = loiter_nav->get_roll();
plane.nav_pitch_cd = loiter_nav->get_pitch();
if (now - quadplane.last_pidz_init_ms < (uint32_t)quadplane.transition_time_ms*2 && !quadplane.tailsitter.enabled()) {
// we limit pitch during initial transition
float pitch_limit_cd = linear_interpolate(quadplane.loiter_initial_pitch_cd, quadplane.aparm.angle_max,
now,
quadplane.last_pidz_init_ms, quadplane.last_pidz_init_ms+quadplane.transition_time_ms*2);
if (plane.nav_pitch_cd > pitch_limit_cd) {
plane.nav_pitch_cd = pitch_limit_cd;
pos_control->set_externally_limited_xy();
}
if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,

115
ArduPlane/quadplane.cpp
View File

@ -429,6 +429,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @Path: tiltrotor.cpp
AP_SUBGROUPINFO(tiltrotor, "TILT_", 27, QuadPlane, Tiltrotor),
// @Param: BACKTRANS_MS
// @DisplayName: SLT and Tiltrotor back transition pitch limit duration
// @Description: Pitch angle will increase from 0 to angle max over this duration when switching into VTOL flight in a postion control mode. 0 Disables.
// @Units: ms
// @Range: 0 10000
AP_GROUPINFO("BACKTRANS_MS", 28, QuadPlane, back_trans_pitch_limit_ms, 3000),
AP_GROUPEND
};
@ -1555,10 +1562,15 @@ void SLT_Transition::update()
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
motors->output();
}
last_fw_mode_ms = now;
last_fw_nav_pitch_cd = plane.nav_pitch_cd;
return;
}
quadplane.motors_output();
last_fw_mode_ms = now;
last_fw_nav_pitch_cd = plane.nav_pitch_cd;
}
void SLT_Transition::VTOL_update()
@ -2326,27 +2338,8 @@ void QuadPlane::vtol_position_controller(void)
plane.nav_roll_cd = pos_control->get_roll_cd();
plane.nav_pitch_cd = pos_control->get_pitch_cd();
if (!tailsitter.enabled()) {
/*
limit the pitch down with an expanding envelope. This
prevents the velocity controller demanding nose down during
the initial slowdown if the target velocity curve is higher
than the actual velocity curve (for a high drag
aircraft). Nose down will cause a lot of downforce on the
wings which will draw a lot of current and also cause the
aircraft to lose altitude rapidly.pitch limit varies also with speed
to prevent inability to progress to position if moving from a loiter
to landing
*/
float minlimit_cd = linear_interpolate(-300, MAX(-aparm.angle_max,plane.aparm.pitch_limit_min_cd),
poscontrol.time_since_state_start_ms(),
0, 5000);
if (plane.nav_pitch_cd < minlimit_cd) {
plane.nav_pitch_cd = minlimit_cd;
// tell the pos controller we have limited the pitch to
// stop integrator buildup
pos_control->set_externally_limited_xy();
}
if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// call attitude controller
@ -2399,6 +2392,10 @@ void QuadPlane::vtol_position_controller(void)
plane.nav_roll_cd = pos_control->get_roll_cd();
plane.nav_pitch_cd = pos_control->get_pitch_cd();
if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
@ -2651,16 +2648,21 @@ void QuadPlane::waypoint_controller(void)
*/
// run wpnav controller
wp_nav->update_wpnav();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
wp_nav->get_yaw(),
true);
// nav roll and pitch are controller by loiter controller
// nav roll and pitch are controller by waypoint controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
pos_control->set_externally_limited_xy();
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
wp_nav->get_yaw(),
true);
// climb based on altitude error
set_climb_rate_cms(assist_climb_rate_cms(), false);
run_z_controller();
@ -3692,6 +3694,61 @@ bool SLT_Transition::active() const
return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER));
}
bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_cd)
{
if (quadplane.back_trans_pitch_limit_ms <= 0) {
// disabled
return false;
}
uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms;
const uint32_t now = AP_HAL::millis();
if (now - last_fw_mode_ms > limit_time_ms) {
// past transition period, nothing to do
return false;
}
// we limit pitch during initial transition
float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd),
now,
last_fw_mode_ms, last_fw_mode_ms+limit_time_ms);
if (pitch_cd > max_limit_cd) {
pitch_cd = max_limit_cd;
return true;
}
/*
limit the pitch down with an expanding envelope. This
prevents the velocity controller demanding nose down during
the initial slowdown if the target velocity curve is higher
than the actual velocity curve (for a high drag
aircraft). Nose down will cause a lot of downforce on the
wings which will draw a lot of current and also cause the
aircraft to lose altitude rapidly.pitch limit varies also with speed
to prevent inability to progress to position if moving from a loiter
to landing
*/
float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd),
now,
last_fw_mode_ms, last_fw_mode_ms+limit_time_ms);
if (plane.nav_pitch_cd < min_limit_cd) {
plane.nav_pitch_cd = min_limit_cd;
return true;
}
return false;
}
void SLT_Transition::force_transistion_complete() {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
last_fw_mode_ms = AP_HAL::millis();
last_fw_nav_pitch_cd = plane.nav_pitch_cd;
};
MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
{
if (quadplane.in_vtol_mode()) {

1
ArduPlane/quadplane.h
View File

@ -273,6 +273,7 @@ private:
float transition_threshold(void);
AP_Int16 transition_time_ms;
AP_Int16 back_trans_pitch_limit_ms;
// transition deceleration, m/s/s
AP_Float transition_decel;

14
ArduPlane/transition.h
View File

@ -50,6 +50,8 @@ public:
virtual MAV_VTOL_STATE get_mav_vtol_state() const = 0;
virtual bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) { return false; }
protected:
// refences for convenience
@ -69,11 +71,7 @@ public:
void VTOL_update() override;
void force_transistion_complete() override {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
};
void force_transistion_complete() override;
bool complete() const override { return transition_state == TRANSITION_DONE; }
@ -91,6 +89,8 @@ public:
MAV_VTOL_STATE get_mav_vtol_state() const override;
bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) override;
protected:
enum {
@ -106,5 +106,9 @@ protected:
// last throttle value when active
float last_throttle;
// time and pitch angle whe last in a vtol or FW control mode
uint32_t last_fw_mode_ms;
int32_t last_fw_nav_pitch_cd;
};