diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index f3735eafd1..4019b006a0 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -115,10 +115,10 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) // check throttle is above, below or in the deadband if (throttle_control < deadband_bottom) { // below the deadband - desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom; + desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom; } else if (throttle_control > deadband_top) { // above the deadband - desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top); + desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top); } else { // must be in the deadband desired_rate = 0.0f; @@ -200,3 +200,13 @@ void Sub::rotate_body_frame_to_NE(float &x, float &y) x = ne_x; y = ne_y; } + +// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value. +uint16_t Sub::get_pilot_speed_dn() +{ + if (g.pilot_speed_dn == 0) { + return abs(g.pilot_speed_up); + } else { + return abs(g.pilot_speed_dn); + } +} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index cab89cab3a..ddc828f5e0 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -201,14 +201,23 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), - // @Param: PILOT_VELZ_MAX - // @DisplayName: Pilot maximum vertical speed - // @Description: The maximum vertical velocity the pilot may request in cm/s + // @Param: PILOT_SPEED_UP + // @DisplayName: Pilot maximum vertical ascending speed + // @Description: The maximum vertical ascending velocity the pilot may request in cm/s // @Units: cm/s // @Range: 50 500 // @Increment: 10 // @User: Standard - GSCALAR(pilot_velocity_z_max, "PILOT_VELZ_MAX", PILOT_VELZ_MAX), + GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), + + // @Param: PILOT_SPEED_DN + // @DisplayName: Pilot maximum vertical descending speed + // @Description: The maximum vertical descending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_speed_dn, "PILOT_SPEED_DN", 0), // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index a023f06916..3031ff6e6c 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -180,7 +180,7 @@ public: k_param_rangefinder_gain, k_param_wp_yaw_behavior = 170, k_param_xtrack_angle_limit, // Angle limit for crosstrack correction in Auto modes (degrees) - k_param_pilot_velocity_z_max, + k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max k_param_pilot_accel_z, k_param_compass_enabled, k_param_surface_depth, @@ -210,6 +210,7 @@ public: k_param_cam_slew_limit = 237, // deprecated k_param_lights_steps, + k_param_pilot_speed_dn, }; @@ -248,7 +249,8 @@ public: // Waypoints // - AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request + AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request + AP_Int16 pilot_speed_dn; // maximum vertical descending velocity the pilot may request AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request // Throttle diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 91d0cc757a..9010aaf523 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -729,6 +729,8 @@ private: bool surface_init(void); void surface_run(); + uint16_t get_pilot_speed_dn(); + void convert_old_parameters(void); public: diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 2f27450128..d9ab9bc63a 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -17,7 +17,7 @@ bool Sub::althold_init() // initialize vertical speeds and leash lengths // sets the maximum speed up and down returned by position controller - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -36,7 +36,7 @@ void Sub::althold_run() uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds and acceleration - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); if (!motors.armed()) { diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index eabf2488e1..f2e061f24a 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -17,7 +17,7 @@ bool Sub::circle_init() pos_control.set_speed_xy(wp_nav.get_speed_xy()); pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); pos_control.set_jerk_xy_to_default(); - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed @@ -36,7 +36,7 @@ void Sub::circle_run() // update parameters, to allow changing at runtime pos_control.set_speed_xy(wp_nav.get_speed_xy()); pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); // if not armed set throttle to zero and exit immediately diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index ad960bed90..436d3b2ba6 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -77,7 +77,7 @@ void Sub::guided_vel_control_start() guided_mode = Guided_Velocity; // initialize vertical speeds and leash lengths - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); // initialise velocity controller diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 78a9048774..c00a3be197 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -15,7 +15,7 @@ bool Sub::poshold_init() } // initialize vertical speeds and acceleration - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -110,7 +110,7 @@ void Sub::poshold_run() // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // adjust climb rate using rangefinder if (rangefinder_alt_ok()) {