Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Fix before squash

This commit is contained in:
Leonard Hall 2021-05-19 23:38:30 +09:30 committed by Andrew Tridgell
parent 8b3b6cf693
commit 4c3a5c0918
4 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/Attitude.cpp
View File

@ -148,8 +148,8 @@ 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
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());
rangefinder_state.alt_cm - pos_control.get_pos_error_z_down_cm(),
rangefinder_state.alt_cm + pos_control.get_pos_error_z_up_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);

9
ArduSub/control_auto.cpp
View File

@ -148,7 +148,8 @@ void Sub::auto_wp_run()
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
////////////////////////////
@ -242,7 +243,8 @@ void Sub::auto_circle_run()
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
@ -329,7 +331,8 @@ void Sub::auto_loiter_run()
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
// get pilot desired lean angles

8
ArduSub/control_guided.cpp
View File

@ -311,7 +311,8 @@ void Sub::guided_pos_control_run()
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call z-axis position controller (wpnav should have already updated it's alt target)
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
// call attitude controller
@ -416,11 +417,8 @@ void Sub::guided_posvel_control_run()
posvel_vel_target_cms.zero();
}
// calculate dt
float dt = pos_control.get_dt();
// advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * dt;
posvel_pos_target_cm += posvel_vel_target_cms * pos_control.get_dt();
// send position and velocity targets to position controller
pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());

2
ArduSub/control_surface.cpp
View File

@ -28,7 +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());
pos_control.init_z_controller();
return;
}