From 266bd22df3255ba9ed69728627ac59af0d4eda08 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 27 Apr 2021 16:20:06 +0930 Subject: [PATCH] Sub: Use PosControl fixes --- ArduSub/Attitude.cpp | 5 ++- ArduSub/GCS_Mavlink.cpp | 4 +- ArduSub/Log.cpp | 4 +- ArduSub/Sub.cpp | 2 +- ArduSub/control_acro.cpp | 2 +- ArduSub/control_althold.cpp | 24 +++++------ ArduSub/control_auto.cpp | 19 +++----- ArduSub/control_circle.cpp | 15 +++---- ArduSub/control_guided.cpp | 81 +++++++++++++++-------------------- ArduSub/control_manual.cpp | 2 +- ArduSub/control_poshold.cpp | 25 ++++------- ArduSub/control_stabilize.cpp | 2 +- ArduSub/control_surface.cpp | 13 +++--- ArduSub/system.cpp | 2 - 14 files changed, 85 insertions(+), 115 deletions(-) diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 695c90e0f2..f50f3cd364 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -147,8 +147,9 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al } // do not let target altitude get too far from current altitude above ground - // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition - target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z()); + target_rangefinder_alt = constrain_float(target_rangefinder_alt, + rangefinder_state.alt_cm - pos_control.get_pos_error_min_z_cm(), + rangefinder_state.alt_cm + pos_control.get_pos_error_max_z_cm()); // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index a8bbf515d0..9e1e8c3f0d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -92,7 +92,7 @@ void GCS_MAVLINK_Sub::send_nav_controller_output() const targets.z * 1.0e-2f, sub.wp_nav.get_wp_bearing_to_destination() * 1.0e-2f, MIN(sub.wp_nav.get_wp_distance_to_destination() * 1.0e-2f, UINT16_MAX), - sub.pos_control.get_alt_error() * 1.0e-2f, + sub.pos_control.get_pos_error_z_cm() * 1.0e-2f, 0, 0); } @@ -679,7 +679,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) */ if (!pos_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD - sub.pos_control.set_alt_target(packet.alt*100); + sub.pos_control.set_pos_target_z_cm(packet.alt*100); break; } diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 1d3bea14eb..f2c8dabb65 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -38,13 +38,13 @@ void Sub::Log_Write_Control_Tuning() angle_boost : attitude_control.angle_boost(), throttle_out : motors.get_throttle(), throttle_hover : motors.get_throttle_hover(), - desired_alt : pos_control.get_alt_target() / 100.0f, + desired_alt : pos_control.get_pos_target_z_cm() / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : barometer.get_altitude(), desired_rangefinder_alt : (int16_t)target_rangefinder_alt, rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, - target_climb_rate : (int16_t)pos_control.get_vel_target_z(), + target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), climb_rate : climb_rate }; logger.WriteBlock(&pkt, sizeof(pkt)); diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index a8cdc99371..7a087863e1 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -34,7 +34,7 @@ Sub::Sub() inertial_nav(ahrs), ahrs_view(ahrs, ROTATION_NONE), attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS), - pos_control(ahrs_view, inertial_nav, motors, attitude_control), + pos_control(ahrs_view, inertial_nav, motors, attitude_control, MAIN_LOOP_SECONDS), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 48047f4a43..0ea73b30b2 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -9,7 +9,7 @@ bool Sub::acro_init() { // set target altitude to zero for reporting - pos_control.set_alt_target(0); + pos_control.set_pos_target_z_cm(0); // attitude hold inputs become thrust inputs in acro mode // set to neutral to prevent chaotic behavior (esp. roll/pitch) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index d6fcf7ccda..268c06a170 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -12,14 +12,15 @@ bool Sub::althold_init() return false; } - // initialize vertical speeds and leash lengths + // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + + // initialize vertical speeds and acceleration + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control.init_z_controller(); last_pilot_heading = ahrs.yaw_sensor; @@ -33,15 +34,14 @@ void Sub::althold_run() uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds and acceleration - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + pos_control.set_max_speed_accel_z(-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,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); - pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); + pos_control.relax_z_controller(motors.get_throttle_hover()); last_pilot_heading = ahrs.yaw_sensor; return; } @@ -111,14 +111,14 @@ void Sub::control_depth() { // output pilot's throttle attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); // reset z targets to current values - pos_control.relax_alt_hold_controllers(); + pos_control.relax_z_controller(channel_throttle->norm_input()); engageStopZ = true; lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z()); } else { // hold z if (ap.at_bottom) { - pos_control.relax_alt_hold_controllers(); // clear velocity and position targets - pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom + pos_control.init_z_controller(); + pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom } // Detects a zero derivative @@ -127,7 +127,7 @@ void Sub::control_depth() { // or smaller input signals if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) { engageStopZ = false; - pos_control.relax_alt_hold_controllers(); + pos_control.init_z_controller(); } pos_control.update_z_controller(); diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 3a759b672b..540d2927b1 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -514,9 +514,9 @@ float Sub::get_auto_heading() float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); // Bearing from current position towards intermediate position target (centidegrees) - const Vector2f target_vel_xy{pos_control.get_vel_target().x, pos_control.get_vel_target().y}; + const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y}; float angle_error = 0.0f; - if (target_vel_xy.length() >= pos_control.get_max_speed_xy() * 0.1f) { + if (target_vel_xy.length() >= pos_control.get_max_speed_xy_cms() * 0.1f) { const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; angle_error = wrap_180_cd(desired_angle_cd - track_bearing); } @@ -565,15 +565,10 @@ bool Sub::auto_terrain_recover_start() loiter_nav.init_target(); // Reset z axis controller - pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); + pos_control.relax_z_controller(motors.get_throttle_hover()); - // initialize vertical speeds and leash lengths - pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); - pos_control.set_max_accel_z(wp_nav.get_accel_z()); - - // Reset vertical position and velocity targets - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + // initialize vertical maximum speeds and acceleration + pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -616,7 +611,7 @@ void Sub::auto_terrain_recover_run() // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // Reset alt hold targets + pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets } // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled @@ -663,7 +658,7 @@ void Sub::auto_terrain_recover_run() ///////////////////// // update z target // - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, true); + pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate, true); pos_control.update_z_controller(); //////////////////////////// diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 13a504f083..0dec0cd2b8 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -14,10 +14,8 @@ bool Sub::circle_init() circle_pilot_yaw_override = false; // initialize speeds and accelerations - pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy()); - pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed circle_nav.init(); @@ -33,10 +31,8 @@ void Sub::circle_run() float target_climb_rate = 0; // update parameters, to allow changing at runtime - pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy()); - pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_max_speed_accel_z(-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()) { @@ -45,7 +41,6 @@ void Sub::circle_run() // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); - pos_control.set_alt_target_to_current_alt(); return; } @@ -83,6 +78,6 @@ void Sub::circle_run() } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); pos_control.update_z_controller(); } diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index b607a96b93..643bfd4d56 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -73,12 +73,12 @@ void Sub::guided_vel_control_start() // set guided_mode to velocity controller guided_mode = Guided_Velocity; - // initialize vertical speeds and leash lengths - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + // initialize vertical maximum speeds and acceleration + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller - pos_control.init_vel_controller_xyz(); + pos_control.init_z_controller(); + pos_control.init_xy_controller(); } // initialise guided mode's posvel controller @@ -87,22 +87,12 @@ void Sub::guided_posvel_control_start() // set guided_mode to velocity controller guided_mode = Guided_PosVel; - pos_control.init_xy_controller(); - - // set speed and acceleration from wpnav's speed and acceleration - pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy()); - pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration()); - - const Vector3f& curr_pos = inertial_nav.get_position(); - const Vector3f& curr_vel = inertial_nav.get_velocity(); - - // set target position and velocity to current position and velocity - pos_control.set_xy_target(curr_pos.x, curr_pos.y); - pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y); - // set vertical speed and acceleration - pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); - pos_control.set_max_accel_z(wp_nav.get_accel_z()); + pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + + // initialise velocity controller + pos_control.init_z_controller(); + pos_control.init_xy_controller(); // pilot always controls yaw set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -115,12 +105,10 @@ void Sub::guided_angle_control_start() guided_mode = Guided_Angle; // set vertical speed and acceleration - pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); - pos_control.set_max_accel_z(wp_nav.get_accel_z()); + pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + // initialise velocity controller + pos_control.init_z_controller(); // initialise targets guided_angle_state.update_time_ms = AP_HAL::millis(); @@ -204,7 +192,7 @@ void Sub::guided_set_velocity(const Vector3f& velocity) vel_update_time_ms = AP_HAL::millis(); // set position controller velocity target - pos_control.set_desired_velocity(velocity); + pos_control.set_vel_desired_cms(velocity); } // set guided mode posvel target @@ -229,7 +217,8 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto posvel_pos_target_cm = destination; posvel_vel_target_cms = velocity; - pos_control.set_pos_target(posvel_pos_target_cm); + pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f()); + pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f()); // log target Log_Write_GuidedTarget(guided_mode, destination, velocity); @@ -293,6 +282,9 @@ void Sub::guided_pos_control_run() // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); + // initialise velocity controller + pos_control.init_z_controller(); + pos_control.init_xy_controller(); return; } @@ -338,12 +330,13 @@ void Sub::guided_vel_control_run() { // ifmotors not enabled set throttle to zero and exit immediately if (!motors.armed()) { - // initialise velocity controller - pos_control.init_vel_controller_xyz(); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); + // initialise velocity controller + pos_control.init_z_controller(); + pos_control.init_xy_controller(); return; } @@ -362,12 +355,13 @@ void Sub::guided_vel_control_run() // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); - if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { - pos_control.set_desired_velocity(Vector3f(0,0,0)); + if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { + pos_control.set_vel_desired_cms(Vector3f(0,0,0)); } // call velocity controller which includes z axis controller - pos_control.update_vel_controller_xyz(); + pos_control.update_xy_controller(); + pos_control.update_z_controller(); float lateral_out, forward_out; translate_pos_control_rp(lateral_out, forward_out); @@ -392,13 +386,13 @@ void Sub::guided_posvel_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { - // set target position and velocity to current position and velocity - pos_control.set_pos_target(inertial_nav.get_position()); - pos_control.set_desired_velocity(Vector3f(0,0,0)); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); + // initialise velocity controller + pos_control.init_z_controller(); + pos_control.init_xy_controller(); return; } @@ -423,22 +417,18 @@ void Sub::guided_posvel_control_run() } // calculate dt - float dt = pos_control.time_since_last_xy_update(); - - // sanity check dt - if (dt >= 0.2f) { - dt = 0.0f; - } + float dt = pos_control.get_dt(); // advance position target using velocity target posvel_pos_target_cm += posvel_vel_target_cms * dt; // send position and velocity targets to position controller - pos_control.set_pos_target(posvel_pos_target_cm); - pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y); + pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f()); + pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f()); // run position controller pos_control.update_xy_controller(); + pos_control.update_z_controller(); float lateral_out, forward_out; translate_pos_control_rp(lateral_out, forward_out); @@ -447,8 +437,6 @@ void Sub::guided_posvel_control_run() motors.set_lateral(lateral_out); motors.set_forward(forward_out); - pos_control.update_z_controller(); - // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot @@ -469,7 +457,8 @@ void Sub::guided_angle_control_run() // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); - pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); + // initialise velocity controller + pos_control.init_z_controller(); return; } @@ -505,7 +494,7 @@ void Sub::guided_angle_control_run() attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); // call position controller - pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); + pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false); pos_control.update_z_controller(); } diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index b68521abc2..93e821740a 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -4,7 +4,7 @@ bool Sub::manual_init() { // set target altitude to zero for reporting - pos_control.set_alt_target(0); + pos_control.set_pos_target_z_cm(0); // attitude hold inputs become thrust inputs in manual mode // set to neutral to prevent chaotic behavior (esp. roll/pitch) diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 39fb548515..e70d3589a3 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -13,22 +13,19 @@ bool Sub::poshold_init() if (!position_ok()) { return false; } - pos_control.init_vel_controller_xyz(); - pos_control.set_desired_velocity_xy(0, 0); - pos_control.set_target_to_stopping_point_xy(); // initialize vertical speeds and acceleration - pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control.set_max_accel_z(g.pilot_accel_z); + pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control.init_xy_controller_stopping_point(); + pos_control.init_z_controller(); // Stop all thrusters attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control.relax_attitude_controllers(); - pos_control.relax_alt_hold_controllers(); + pos_control.relax_z_controller(0.5f); last_pilot_heading = ahrs.yaw_sensor; @@ -46,8 +43,8 @@ void Sub::poshold_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(); - pos_control.set_target_to_stopping_point_xy(); - pos_control.relax_alt_hold_controllers(); + pos_control.init_xy_controller_stopping_point(); + pos_control.relax_z_controller(0.5f); last_pilot_heading = ahrs.yaw_sensor; return; } @@ -63,19 +60,15 @@ void Sub::poshold_run() float lateral_out = 0; float forward_out = 0; - pos_control.set_desired_velocity_xy(0,0); - if (position_ok()) { // Allow pilot to reposition the sub if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { - pos_control.set_target_to_stopping_point_xy(); + pos_control.init_xy_controller_stopping_point(); } translate_pos_control_rp(lateral_out, forward_out); pos_control.update_xy_controller(); } else { - pos_control.init_vel_controller_xyz(); - pos_control.set_desired_velocity_xy(0, 0); - pos_control.set_target_to_stopping_point_xy(); + pos_control.init_xy_controller_stopping_point(); } motors.set_forward(forward_out + pilot_forward); motors.set_lateral(lateral_out + pilot_lateral); diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index ad0118a200..8d135fa82a 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -4,7 +4,7 @@ bool Sub::stabilize_init() { // set target altitude to zero for reporting - pos_control.set_alt_target(0); + pos_control.set_pos_target_z_cm(0); last_pilot_heading = ahrs.yaw_sensor; return true; diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 3b05f40529..42aa2e56a1 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -7,13 +7,11 @@ bool Sub::surface_init() return false; } - // initialize vertical speeds and leash lengths - pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); - pos_control.set_max_accel_z(wp_nav.get_accel_z()); + // initialize vertical speeds and acceleration + pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control.init_z_controller(); return true; @@ -30,6 +28,7 @@ void Sub::surface_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(); + pos_control.relax_z_controller(motors.get_throttle_hover()); return; } @@ -49,13 +48,13 @@ void Sub::surface_run() attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // set target climb rate - float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up()); + float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up_cms()); // record desired climb rate for logging desired_climb_rate = cmb_rate; // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); + pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate, true); pos_control.update_z_controller(); // pilot has control for repositioning diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index f82707508f..9fac8426ee 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -94,8 +94,6 @@ void Sub::init_ardupilot() wp_nav.set_terrain(&terrain); #endif - pos_control.set_dt(MAIN_LOOP_SECONDS); - // init the optical flow sensor #if OPTFLOW == ENABLED init_optflow();