From 6419f811f9edbf07bd1a6cb88bb31dc41dbcdcc2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 14 Nov 2025 10:48:45 +1030 Subject: [PATCH] Sub: NEU to NED renaming - No compiler change --- ArduSub/GCS_MAVLink_Sub.cpp | 2 +- ArduSub/Sub.cpp | 6 ++--- ArduSub/mode_althold.cpp | 14 +++++----- ArduSub/mode_auto.cpp | 30 ++++++++++----------- ArduSub/mode_circle.cpp | 16 ++++++------ ArduSub/mode_guided.cpp | 52 ++++++++++++++++++------------------- ArduSub/mode_poshold.cpp | 24 ++++++++--------- ArduSub/mode_surface.cpp | 12 ++++----- ArduSub/mode_surftrak.cpp | 4 +-- 9 files changed, 80 insertions(+), 80 deletions(-) diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/ArduSub/GCS_MAVLink_Sub.cpp index cf6dc0b3a89..129576035d4 100644 --- a/ArduSub/GCS_MAVLink_Sub.cpp +++ b/ArduSub/GCS_MAVLink_Sub.cpp @@ -219,7 +219,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() } } if (g.gcs_pid_mask & 8) { - const AP_PIDInfo &pid_info = sub.pos_control.get_accel_U_pid().get_pid_info(); + const AP_PIDInfo &pid_info = sub.pos_control.D_get_accel_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, pid_info.target*0.01f, -(ahrs.get_accel_ef().z + GRAVITY_MSS), diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index da37603ba32..bc3b9218de2 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -232,7 +232,7 @@ void Sub::ten_hz_logging_loop() logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_U_pid().get_pid_info()); + logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info()); } } if (should_log(MASK_LOG_MOTBATT)) { @@ -269,7 +269,7 @@ void Sub::twentyfive_hz_logging() logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info()); - logger.Write_PID(LOG_PIDA_MSG, pos_control.get_accel_U_pid().get_pid_info()); + logger.Write_PID(LOG_PIDA_MSG, pos_control.D_get_accel_pid().get_pid_info()); } } @@ -342,7 +342,7 @@ void Sub::one_hz_loop() set_likely_flying(hal.util->get_soft_armed()); attitude_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); - pos_control.get_accel_U_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + pos_control.D_get_accel_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } void Sub::read_AHRS() diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index 152b920234c..125c248b3b7 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -9,11 +9,11 @@ bool ModeAlthold::init(bool ignore_checks) { // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_U_controller(); + position_control->D_init_controller(); sub.last_pilot_heading = ahrs.yaw_sensor; @@ -35,14 +35,14 @@ void ModeAlthold::run_pre() // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control->set_throttle_out(0.5,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->relax_U_controller(motors.get_throttle_hover()); + position_control->D_relax_controller(motors.get_throttle_hover()); sub.last_pilot_heading = ahrs.yaw_sensor; return; } @@ -125,6 +125,6 @@ void ModeAlthold::control_depth() { } } - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms); + position_control->D_update_controller(); } diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index dd2ff876bc8..2c1eef6db29 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -148,7 +148,7 @@ void ModeAuto::auto_wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); //////////////////////////// // update attitude output // @@ -186,17 +186,17 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float sub.circle_nav.set_rate_degs(current_rate); // check our distance from edge of circle - Vector3f circle_edge_neu; + Vector3f circle_edge_neu_cm; float dist_to_edge; - sub.circle_nav.get_closest_point_on_circle_NEU_cm(circle_edge_neu, dist_to_edge); + sub.circle_nav.get_closest_point_on_circle_NEU_cm(circle_edge_neu_cm, dist_to_edge); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle sub.auto_mode = Auto_CircleMoveToEdge; - // convert circle_edge_neu to Location - Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); + // convert circle_edge_neu_cm to Location + Location circle_edge(circle_edge_neu_cm, Location::AltFrame::ABOVE_ORIGIN); // convert altitude to same as command circle_edge.copy_alt_from(circle_center); @@ -246,7 +246,7 @@ void ModeAuto::auto_circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw_cd(), true); @@ -332,7 +332,7 @@ void ModeAuto::auto_loiter_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // get pilot desired lean angles float target_roll, target_pitch; @@ -455,12 +455,12 @@ bool ModeAuto::auto_terrain_recover_start() sub.loiter_nav.init_target(); // Reset z axis controller - position_control->relax_U_controller(motors.get_throttle_hover()); + position_control->D_relax_controller(motors.get_throttle_hover()); // initialize vertical maximum speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -482,8 +482,8 @@ void ModeAuto::auto_terrain_recover_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - sub.loiter_nav.init_target(); // Reset xy target - position_control->relax_U_controller(motors.get_throttle_hover()); // Reset z axis controller + sub.loiter_nav.init_target(); // Reset xy target + position_control->D_relax_controller(motors.get_throttle_hover()); // Reset z axis controller return; } @@ -510,7 +510,7 @@ void ModeAuto::auto_terrain_recover_run() // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - position_control->relax_U_controller(motors.get_throttle_hover()); // Reset alt hold targets + position_control->D_relax_controller(motors.get_throttle_hover()); // Reset alt hold targets } // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled @@ -561,8 +561,8 @@ void ModeAuto::auto_terrain_recover_run() ///////////////////// // update z target // - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate); + position_control->D_update_controller(); //////////////////////////// // update angular targets // diff --git a/ArduSub/mode_circle.cpp b/ArduSub/mode_circle.cpp index 13ac49e50da..27ad85d6788 100644 --- a/ArduSub/mode_circle.cpp +++ b/ArduSub/mode_circle.cpp @@ -15,10 +15,10 @@ bool ModeCircle::init(bool ignore_checks) // initialize speeds and accelerations // All limits must be positive - position_control->set_max_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_correction_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->NE_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed sub.circle_nav.init(); @@ -35,8 +35,8 @@ void ModeCircle::run() // update parameters, to allow changing at runtime // All limits must be positive - position_control->set_max_speed_accel_NE_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss()); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -83,6 +83,6 @@ void ModeCircle::run() } // update altitude target and call position controller - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate); + position_control->D_update_controller(); } diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index 122bfbc064a..36c03366ce3 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -105,12 +105,12 @@ void ModeGuided::guided_vel_control_start() // initialize vertical maximum speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); // pilot always controls yaw sub.yaw_rate_only = false; @@ -125,12 +125,12 @@ void ModeGuided::guided_posvel_control_start() // set vertical speed and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); // pilot always controls yaw sub.yaw_rate_only = false; @@ -145,11 +145,11 @@ void ModeGuided::guided_angle_control_start() // set vertical speed and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); - position_control->set_correction_speed_accel_U_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_U_cmss()); + position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); + position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss()); // initialise velocity controller - position_control->init_U_controller(); + position_control->D_init_controller(); // initialise targets guided_angle_state.update_time_ms = AP_HAL::millis(); @@ -501,7 +501,7 @@ void ModeGuided::guided_pos_control_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - position_control->update_U_controller(); + position_control->D_update_controller(); // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { @@ -532,8 +532,8 @@ void ModeGuided::guided_vel_control_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); return; } @@ -562,12 +562,12 @@ void ModeGuided::guided_vel_control_run() position_control->set_vel_desired_NEU_cms(Vector3f(0,0,0)); } - position_control->stop_pos_NE_stabilisation(); + position_control->NE_stop_pos_stabilisation(); // call velocity controller which includes z axis controller - position_control->update_NE_controller(); + position_control->NE_update_controller(); - position_control->set_pos_target_U_from_climb_rate_cms(position_control->get_vel_desired_NEU_cms().z); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(position_control->get_vel_desired_NEU_cms().z); + position_control->D_update_controller(); float lateral_out, forward_out; sub.translate_pos_control_rp(lateral_out, forward_out); @@ -605,8 +605,8 @@ void ModeGuided::guided_posvel_control_run() attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); - position_control->init_NE_controller(); + position_control->D_init_controller(); + position_control->NE_init_controller(); return; } @@ -646,8 +646,8 @@ void ModeGuided::guided_posvel_control_run() posvel_pos_target_cm.z = pz; // run position controller - position_control->update_NE_controller(); - position_control->update_U_controller(); + position_control->NE_update_controller(); + position_control->D_update_controller(); float lateral_out, forward_out; sub.translate_pos_control_rp(lateral_out, forward_out); @@ -685,7 +685,7 @@ void ModeGuided::guided_angle_control_run() attitude_control->set_throttle_out(0.0f,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); // initialise velocity controller - position_control->init_U_controller(); + position_control->D_init_controller(); return; } @@ -721,8 +721,8 @@ void ModeGuided::guided_angle_control_run() attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_in, pitch_in, yaw_in, true); // call position controller - position_control->set_pos_target_U_from_climb_rate_cms(climb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(climb_rate_cms); + position_control->D_update_controller(); } // Guided Limit code @@ -816,7 +816,7 @@ float ModeGuided::get_auto_heading() // Bearing from current position towards intermediate position target (centidegrees) const Vector2f target_vel_ne_cms = position_control->get_vel_target_NEU_cms().xy(); float angle_error = 0.0f; - if (target_vel_ne_cms.length() >= position_control->get_max_speed_NE_cms() * 0.1f) { + if (target_vel_ne_cms.length() >= position_control->NE_get_max_speed_cms() * 0.1f) { const float desired_angle_cd = degrees(target_vel_ne_cms.angle()) * 100.0f; angle_error = wrap_180_cd(desired_angle_cd - track_bearing); } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index 557eca7b96f..ba51e4d4bd7 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -16,19 +16,19 @@ bool ModePoshold::init(bool ignore_checks) // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_NE_cm(g.pilot_speed, g.pilot_accel_z); - position_control->set_correction_speed_accel_NE_cm(g.pilot_speed, g.pilot_accel_z); - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->NE_set_max_speed_accel_cm(g.pilot_speed, g.pilot_accel_z); + position_control->NE_set_correction_speed_accel_cm(g.pilot_speed, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_NE_controller_stopping_point(); - position_control->init_U_controller(); + position_control->NE_init_controller_stopping_point(); + position_control->D_init_controller(); // Stop all thrusters attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->relax_U_controller(0.5f); + position_control->D_relax_controller(0.5f); sub.last_pilot_heading = ahrs.yaw_sensor; @@ -46,8 +46,8 @@ void ModePoshold::run() // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->init_NE_controller_stopping_point(); - position_control->relax_U_controller(0.5f); + position_control->NE_init_controller_stopping_point(); + position_control->D_relax_controller(0.5f); sub.last_pilot_heading = ahrs.yaw_sensor; return; } @@ -108,9 +108,9 @@ void ModePoshold::control_horizontal() { }; if (sub.position_ok()) { - if (!position_control->is_active_NE()) { + if (!position_control->NE_is_active()) { // the xy controller timed out, re-initialize - position_control->init_NE_controller_stopping_point(); + position_control->NE_init_controller_stopping_point(); } // convert to the earth frame and set target rates @@ -121,7 +121,7 @@ void ModePoshold::control_horizontal() { sub.translate_pos_control_rp(lateral_out, forward_out); // update the xy controller - position_control->update_NE_controller(); + position_control->NE_update_controller(); } else if (g.pilot_speed > 0) { // allow the pilot to reposition manually forward_out = body_rates_cms.x / (float)g.pilot_speed; diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index 1e312c6123d..9561aea4f40 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -7,11 +7,11 @@ bool ModeSurface::init(bool ignore_checks) // initialize vertical speeds and acceleration // All limits must be positive - position_control->set_max_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - position_control->set_correction_speed_accel_U_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - position_control->init_U_controller(); + position_control->D_init_controller(); return true; @@ -27,7 +27,7 @@ void ModeSurface::run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - position_control->init_U_controller(); + position_control->D_init_controller(); return; } @@ -55,8 +55,8 @@ void ModeSurface::run() float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms()); // update altitude target and call position controller - position_control->set_pos_target_U_from_climb_rate_cms(cmb_rate_cms); - position_control->update_U_controller(); + position_control->D_set_pos_target_from_climb_rate_cms(cmb_rate_cms); + position_control->D_update_controller(); } // pilot has control for repositioning motors.set_forward(channel_forward->norm_input()); diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp index 6f0404271ed..856eb54f3f4 100644 --- a/ArduSub/mode_surftrak.cpp +++ b/ArduSub/mode_surftrak.cpp @@ -139,10 +139,10 @@ void ModeSurftrak::control_range() { } // Set the target altitude from the climb rate and the terrain offset - position_control->set_pos_target_U_from_climb_rate_cms(target_climb_rate_cms); + position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms); // Run the PID controllers - position_control->update_U_controller(); + position_control->D_update_controller(); } /*