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

Sub: update control_poshold to use AC_PosControlSub

This commit is contained in:
Willian Galvani 2020-08-03 00:32:30 -03:00 committed by Jacob Walser
parent 7fa8a455d7
commit 385f440d90
1 changed files with 28 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
ArduSub/control_poshold.cpp
View File

@ -13,6 +13,9 @@ 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);
@ -22,10 +25,10 @@ bool Sub::poshold_init()
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target();
// 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();
last_pilot_heading = ahrs.yaw_sensor;
@ -37,24 +40,21 @@ bool Sub::poshold_init()
void Sub::poshold_run()
{
uint32_t tnow = AP_HAL::millis();
// if not armed set throttle to zero and exit immediately
// When unarmed, disable motors and stabilization
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target();
attitude_control.set_throttle_out(0,true,g.throttle_filt);
// 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.relax_alt_hold_controllers(motors.get_throttle_hover());
pos_control.set_target_to_stopping_point_xy();
pos_control.relax_alt_hold_controllers();
last_pilot_heading = ahrs.yaw_sensor;
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run loiter controller
loiter_nav.update();
///////////////////////
// update xy outputs //
float pilot_lateral = channel_lateral->norm_input();
@ -63,19 +63,22 @@ void Sub::poshold_run()
float lateral_out = 0;
float forward_out = 0;
// Allow pilot to reposition the sub
if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
lateral_out = pilot_lateral;
forward_out = pilot_forward;
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); // initialize target to current position after repositioning
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();
}
translate_pos_control_rp(lateral_out, forward_out);
pos_control.update_xy_controller();
} else {
translate_wpnav_rp(lateral_out, forward_out);
pos_control.init_vel_controller_xyz();
pos_control.set_desired_velocity_xy(0, 0);
pos_control.set_target_to_stopping_point_xy();
}
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
motors.set_forward(forward_out + pilot_forward);
motors.set_lateral(lateral_out + pilot_lateral);
/////////////////////
// Update attitude //
@ -109,21 +112,7 @@ void Sub::poshold_run()
}
}
///////////////////
// Update z axis //
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// call z axis position controller
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else {
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
pos_control.update_z_controller();
control_depth();
}
#endif // POSHOLD_ENABLED == ENABLED