mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
Plane: added LEVEL_ROLL_LIMIT parameter
this replaces both TKOFF_HEAD_HOLD and RUDDER_STEER, allowing users to instead select a roll limit for takeoff and landing
This commit is contained in:
parent
575f346e85
commit
70a186464b
@ -757,7 +757,8 @@ static void fast_loop()
|
||||
#endif
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
// calculates roll, pitch and yaw demands from navigation demands
|
||||
// --------------------------------------------------------------
|
||||
update_current_flight_mode();
|
||||
|
||||
// apply desired roll, pitch and yaw to the plane
|
||||
@ -1007,10 +1008,15 @@ static void update_current_flight_mode(void)
|
||||
|
||||
switch(nav_command_ID) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
if (hold_course_cd != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) {
|
||||
calc_nav_roll();
|
||||
} else {
|
||||
if (hold_course_cd == -1) {
|
||||
// we don't yet have a heading to hold - just level
|
||||
// the wings until we get up enough speed to get a GPS heading
|
||||
nav_roll_cd = 0;
|
||||
} else {
|
||||
calc_nav_roll();
|
||||
// during takeoff use the level flight roll limit to
|
||||
// prevent large course corrections
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
||||
}
|
||||
|
||||
if (alt_control_airspeed()) {
|
||||
@ -1040,13 +1046,13 @@ static void update_current_flight_mode(void)
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LAND:
|
||||
if (g.rudder_steer == 0 || !land_complete) {
|
||||
calc_nav_roll();
|
||||
} else {
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
if (land_complete) {
|
||||
// during final approach constrain roll to the range
|
||||
// allowed for level flight
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
||||
|
||||
// hold pitch constant in final approach
|
||||
nav_pitch_cd = g.land_pitch_cd;
|
||||
} else {
|
||||
|
@ -74,13 +74,14 @@ public:
|
||||
k_param_land_flare_alt,
|
||||
k_param_land_flare_sec,
|
||||
k_param_crosstrack_min_distance, // unused
|
||||
k_param_rudder_steer,
|
||||
k_param_rudder_steer, // unused
|
||||
k_param_throttle_nudge,
|
||||
k_param_alt_offset,
|
||||
k_param_ins, // libraries/AP_InertialSensor variables
|
||||
k_param_takeoff_throttle_min_speed,
|
||||
k_param_takeoff_throttle_min_accel,
|
||||
k_param_takeoff_heading_hold,
|
||||
k_param_takeoff_heading_hold, // unused
|
||||
k_param_level_roll_limit,
|
||||
k_param_hil_servos,
|
||||
k_param_vtail_output,
|
||||
k_param_nav_controller,
|
||||
@ -362,10 +363,9 @@ public:
|
||||
AP_Int8 battery_curr_pin;
|
||||
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
|
||||
AP_Int8 stick_mixing;
|
||||
AP_Int8 rudder_steer;
|
||||
AP_Float takeoff_throttle_min_speed;
|
||||
AP_Float takeoff_throttle_min_accel;
|
||||
AP_Int8 takeoff_heading_hold;
|
||||
AP_Int8 level_roll_limit;
|
||||
|
||||
// RC channels
|
||||
RC_Channel channel_roll;
|
||||
|
@ -106,19 +106,14 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: User
|
||||
GSCALAR(takeoff_throttle_min_accel, "TKOFF_THR_MINACC", 0),
|
||||
|
||||
// @Param: TKOFF_HEAD_HOLD
|
||||
// @DisplayName: Auto takeoff heading hold
|
||||
// @Description: This controls whether APM tries to hold a constant heading during automatic takeoff. The default is 1, which means that it remembers the heading when auto takeoff is triggered, and tries to hold that heading until the target altitude is reached. This is the correct setting for wheeled takeoff. For a hand launch, it may be better to set this to 0, which will tell APM to hold the wings level, but not attempt to hold a particular heading. That can prevent the aircraft from rolling too much on takeoff, which can cause a stall.
|
||||
// @Values: 0:NoHeadingHold,1:HeadingHold
|
||||
// @Param: LEVEL_ROLL_LIMIT
|
||||
// @DisplayName: Level flight roll limit
|
||||
// @Description: This controls the maximum bank angle in degrees during flight modes where level flight is desired, such as in the final stages of landing, and during auto takeoff. This should be a small angle (such as 5 degrees) to prevent a wing hitting the runway during takeoff or landing. Setting this to zero will completely disable heading hold on auto takeoff and final landing approach.
|
||||
// @Units: degrees
|
||||
// @Range: 0 45
|
||||
// @Increment: 1
|
||||
// @User: User
|
||||
GSCALAR(takeoff_heading_hold, "TKOFF_HEAD_HOLD", 1),
|
||||
|
||||
// @Param: RUDDER_STEER
|
||||
// @DisplayName: Rudder steering on takeoff and landing
|
||||
// @Description: When enabled, only rudder will be used for steering during takeoff and landing, with the ailerons used to hold the plane level
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: User
|
||||
GSCALAR(rudder_steer, "RUDDER_STEER", 0),
|
||||
GSCALAR(level_roll_limit, "LEVEL_ROLL_LIMIT", 5),
|
||||
|
||||
// @Param: land_pitch_cd
|
||||
// @DisplayName: Landing Pitch
|
||||
|
@ -299,7 +299,7 @@ static void do_loiter_time()
|
||||
static bool verify_takeoff()
|
||||
{
|
||||
if (ahrs.yaw_initialised()) {
|
||||
if (hold_course_cd == -1 && g.takeoff_heading_hold != 0) {
|
||||
if (hold_course_cd == -1) {
|
||||
// save our current course to take off
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd);
|
||||
|
@ -69,8 +69,8 @@ static void navigate()
|
||||
// update total loiter angle
|
||||
loiter_angle_update();
|
||||
|
||||
// control mode specific updates to nav_bearing
|
||||
// --------------------------------------------
|
||||
// control mode specific updates to navigation demands
|
||||
// ---------------------------------------------------
|
||||
update_navigation();
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user