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

Sub: tweak depth holding behavior in stabilize/althold/poshold

This commit is contained in:
Willian Galvani 2022-09-27 18:09:00 -03:00
parent b04a11ec95
commit 94f3079ca1
3 changed files with 28 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.h
View File

@ -631,6 +631,7 @@ private:
bool get_wp_distance_m(float &distance) const override;
bool get_wp_bearing_deg(float &bearing) const override;
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
float stopping_distance();
enum Failsafe_Action {
Failsafe_Action_None = 0,

24
ArduSub/control_althold.cpp
View File

@ -14,11 +14,15 @@ bool Sub::althold_init()
// initialize vertical maximum speeds and acceleration
// sets the maximum speed up and down returned by position controller
attitude_control.set_throttle_out(0.75, true, 100.0);
pos_control.init_z_controller();
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
attitude_control.relax_attitude_controllers();
// initialise position and desired velocity
pos_control.init_z_controller_stopping_point();
float pos = stopping_distance();
float zero = 0;
pos_control.input_pos_vel_accel_z(pos, zero, zero);
if(prev_control_mode != control_mode_t::STABILIZE) {
last_roll = 0;
@ -31,6 +35,13 @@ bool Sub::althold_init()
}
float Sub::stopping_distance() {
const float curr_pos_z = inertial_nav.get_position().z;
float curr_vel_z = inertial_nav.get_velocity().z;
float distance = - (curr_vel_z * curr_vel_z) / (2 * g.pilot_accel_z);
return curr_pos_z + distance;
}
void Sub::handle_attitude()
{
uint32_t tnow = AP_HAL::millis();
@ -86,9 +97,12 @@ void Sub::althold_run()
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();
pos_control.relax_z_controller(motors.get_throttle_hover());
attitude_control.set_throttle_out(0.75,true,100.0);
pos_control.init_z_controller();
// initialise position and desired velocity
float pos = stopping_distance();
float zero = 0;
pos_control.input_pos_vel_accel_z(pos, zero, zero);
last_roll = 0;
last_pitch = 0;
last_pilot_heading = ahrs.yaw_sensor;

13
ArduSub/control_poshold.cpp
View File

@ -22,12 +22,15 @@ bool Sub::poshold_init()
// initialise position and desired velocity
pos_control.init_xy_controller_stopping_point();
// Stop all thrusters
attitude_control.set_throttle_out(0.75,true,100.0);
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_z_controller(0.5f);
// initialise position and desired velocity
float pos = stopping_distance();
float zero = 0;
pos_control.input_pos_vel_accel_z(pos, zero, zero);
last_pilot_heading = ahrs.yaw_sensor;
@ -43,7 +46,7 @@ void Sub::poshold_run()
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.5f ,true, g.throttle_filt);
attitude_control.set_throttle_out(0.75f ,true, g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.relax_velocity_controller_xy();
pos_control.relax_z_controller(0.5f);