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:
priseborough 2013-04-12 20:27:56 +10:00 committed by Andrew Tridgell
parent 575f346e85
commit 70a186464b
5 changed files with 29 additions and 28 deletions

View File

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

View File

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

View File

@ -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

View File

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

View File

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