mirror of https://github.com/ArduPilot/ardupilot
Plane: implement RTL_CLIMB_MIN parameter
this allows for an initial climb on RTL where roll is limited to LEVEL_ROLL_LIMIT
This commit is contained in:
parent
939331ae8d
commit
8b21f5117a
|
@ -580,8 +580,17 @@ void Plane::update_alt()
|
|||
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
|
||||
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
|
||||
}
|
||||
|
||||
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
|
||||
|
||||
float target_alt = relative_target_altitude_cm();
|
||||
|
||||
if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
|
||||
// ensure we do the initial climb in RTL. We add an extra
|
||||
// 10m in the demanded height to push TECS to climb
|
||||
// quickly
|
||||
target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100);
|
||||
}
|
||||
|
||||
SpdHgt_Controller->update_pitch_throttle(target_alt,
|
||||
target_airspeed_cm,
|
||||
flight_stage,
|
||||
distance_beyond_land_wp,
|
||||
|
|
|
@ -1278,6 +1278,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT),
|
||||
|
||||
// @Param: RTL_CLIMB_MIN
|
||||
// @DisplayName: RTL minimum climb
|
||||
// @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees.
|
||||
// @Units: m
|
||||
// @Range: 0 30
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -577,6 +577,9 @@ public:
|
|||
#endif
|
||||
|
||||
AP_Float fs_ekf_thresh;
|
||||
|
||||
// min initial climb in RTL
|
||||
AP_Int16 rtl_climb_min;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
@ -726,6 +726,10 @@ private:
|
|||
uint32_t last_trim_save;
|
||||
} auto_trim;
|
||||
|
||||
struct {
|
||||
bool done_climb;
|
||||
} rtl;
|
||||
|
||||
// last time home was updated while disarmed
|
||||
uint32_t last_home_update_ms;
|
||||
|
||||
|
|
|
@ -8,6 +8,7 @@ bool ModeRTL::_enter()
|
|||
plane.auto_navigation_mode = true;
|
||||
plane.prev_WP_loc = plane.current_loc;
|
||||
plane.do_RTL(plane.get_RTL_altitude());
|
||||
plane.rtl.done_climb = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
@ -17,5 +18,19 @@ void ModeRTL::update()
|
|||
plane.calc_nav_roll();
|
||||
plane.calc_nav_pitch();
|
||||
plane.calc_throttle();
|
||||
|
||||
if (plane.g2.rtl_climb_min > 0) {
|
||||
/*
|
||||
when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
|
||||
until we have climbed by RTL_CLIMB_MIN meters
|
||||
*/
|
||||
if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) {
|
||||
plane.rtl.done_climb = true;
|
||||
}
|
||||
if (!plane.rtl.done_climb) {
|
||||
plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
|
||||
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue