diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 5a8ff5938db..dc0a9f4cd6d 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -117,6 +117,33 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) } } +// behavior is similar to Sub::get_pilot_desired_climb_rate +float Sub::get_pilot_desired_horizontal_rate(RC_Channel *channel) const +{ + if (failsafe.pilot_input) { + return 0; + } + + // forward and lateral sticks have center trim, unlike throttle + auto control = channel->norm_input(); + + // normalize deadzone + auto dz = (float)g.throttle_deadzone * 2.0f / (float)(channel->get_radio_max() - channel->get_radio_min()); + auto deadband_top = dz * gain; + auto deadband_bottom = -dz * gain; + + if (control < deadband_bottom) { + // below the deadband + return (float)g.pilot_speed * (control - deadband_bottom); + } else if (control > deadband_top) { + // above the deadband + return (float)g.pilot_speed * (control - deadband_top); + } else { + // must be in the deadband + return 0; + } +} + // rotate vector from vehicle's perspective to North-East frame void Sub::rotate_body_frame_to_NE(float &x, float &y) { diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index b31f77cf794..f16b3cb212a 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -175,6 +175,15 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(pilot_speed_dn, "PILOT_SPEED_DN", 0), + // @Param: PILOT_SPEED + // @DisplayName: Pilot maximum horizontal speed + // @Description: The maximum horizontal velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 10 500 + // @Increment: 10 + // @User: Standard + GSCALAR(pilot_speed, "PILOT_SPEED", PILOT_SPEED_DEFAULT), + // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration // @Description: The vertical acceleration used when pilot is controlling the altitude diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index fe176e400e6..e7a9937260e 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -231,6 +231,7 @@ public: k_param_pilot_speed_dn, k_param_rangefinder_signal_min, k_param_surftrak_depth, + k_param_pilot_speed, k_param_vehicle = 257, // vehicle common block of parameters }; @@ -267,9 +268,10 @@ public: // Waypoints // - 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 + 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_speed; // maximum horizontal (xy) 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 db09827d994..0eca1a523f2 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -403,6 +403,7 @@ private: float get_roi_yaw(); float get_look_ahead_yaw(); float get_pilot_desired_climb_rate(float throttle_control); + float get_pilot_desired_horizontal_rate(RC_Channel *channel) const; void rotate_body_frame_to_NE(float &x, float &y); #if HAL_LOGGING_ENABLED // methods for AP_Vehicle: diff --git a/ArduSub/config.h b/ArduSub/config.h index 7e2f5686266..b45c3853469 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -167,10 +167,13 @@ # define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter #endif -// default maximum vertical velocity and acceleration the pilot may request +// default maximum velocities and acceleration the pilot may request #ifndef PILOT_VELZ_MAX # define PILOT_VELZ_MAX 500 // maximum vertical velocity in cm/s #endif +#ifndef PILOT_SPEED_DEFAULT +# define PILOT_SPEED_DEFAULT 200 // maximum horizontal velocity in cm/s while under pilot control +#endif #ifndef PILOT_ACCEL_Z_DEFAULT # define PILOT_ACCEL_Z_DEFAULT 100 // vertical acceleration in cm/s/s while altitude is under pilot control #endif diff --git a/ArduSub/mode.h b/ArduSub/mode.h index ba1ba46d95c..6bd47d55f37 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -430,6 +430,10 @@ protected: const char *name() const override { return "POSHOLD"; } const char *name4() const override { return "POSH"; } Mode::Number number() const override { return Mode::Number::POSHOLD; } + +private: + + void control_horizontal(); }; diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index 479cdcb5435..559aa87f632 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -15,8 +15,8 @@ bool ModePoshold::init(bool ignore_checks) } // initialize vertical speeds and acceleration - position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); - position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_xy(g.pilot_speed, g.pilot_accel_z); + position_control->set_correction_speed_accel_xy(g.pilot_speed, g.pilot_accel_z); position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -54,26 +54,6 @@ void ModePoshold::run() // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - /////////////////////// - // update xy outputs // - float pilot_lateral = channel_lateral->norm_input(); - float pilot_forward = channel_forward->norm_input(); - - float lateral_out = 0; - float forward_out = 0; - - if (sub.position_ok()) { - // Allow pilot to reposition the sub - if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { - position_control->init_xy_controller_stopping_point(); - } - sub.translate_pos_control_rp(lateral_out, forward_out); - position_control->update_xy_controller(); - } else { - position_control->init_xy_controller_stopping_point(); - } - motors.set_forward(forward_out + pilot_forward); - motors.set_lateral(lateral_out + pilot_lateral); ///////////////////// // Update attitude // @@ -103,12 +83,51 @@ void ModePoshold::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - } else { // call attitude controller holding absolute absolute bearing + } else { // call attitude controller holding absolute bearing attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } - // Update z axis // + // update z axis control_depth(); + + // update xy axis + // call this after Sub::get_pilot_desired_climb_rate is called so that THR_DZ is reasonable + control_horizontal(); +} + +void ModePoshold::control_horizontal() { + float lateral_out = 0; + float forward_out = 0; + + // get desired rates in the body frame + Vector2f body_rates_cm_s = { + sub.get_pilot_desired_horizontal_rate(channel_forward), + sub.get_pilot_desired_horizontal_rate(channel_lateral) + }; + + if (sub.position_ok()) { + if (!position_control->is_active_xy()) { + // the xy controller timed out, re-initialize + position_control->init_xy_controller_stopping_point(); + } + + // convert to the earth frame and set target rates + auto earth_rates_cm_s = ahrs.body_to_earth2D(body_rates_cm_s); + position_control->input_vel_accel_xy(earth_rates_cm_s, {0, 0}); + + // convert pos control roll and pitch angles back to lateral and forward efforts + sub.translate_pos_control_rp(lateral_out, forward_out); + + // udpate the xy controller + position_control->update_xy_controller(); + } else if (g.pilot_speed > 0) { + // allow the pilot to reposition manually + forward_out = body_rates_cm_s.x / (float)g.pilot_speed; + lateral_out = body_rates_cm_s.y / (float)g.pilot_speed; + } + + motors.set_forward(forward_out); + motors.set_lateral(lateral_out); } #endif // POSHOLD_ENABLED