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

Sub: Use PosControl fixes

This commit is contained in:
Leonard Hall 2021-04-27 16:20:06 +09:30 committed by Andrew Tridgell
parent bddf6602f0
commit 266bd22df3
14 changed files with 85 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/Attitude.cpp
View File

@ -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);

4
ArduSub/GCS_Mavlink.cpp
View File

@ -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;
}

4
ArduSub/Log.cpp
View File

@ -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));

2
ArduSub/Sub.cpp
View File

@ -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),

2
ArduSub/control_acro.cpp
View File

@ -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)

24
ArduSub/control_althold.cpp
View File

@ -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();

19
ArduSub/control_auto.cpp
View File

@ -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();
////////////////////////////

15
ArduSub/control_circle.cpp
View File

@ -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();
}

81
ArduSub/control_guided.cpp
View File

@ -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();
}

2
ArduSub/control_manual.cpp
View File

@ -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)

25
ArduSub/control_poshold.cpp
View File

@ -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);

2
ArduSub/control_stabilize.cpp
View File

@ -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;

13
ArduSub/control_surface.cpp
View File

@ -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

2
ArduSub/system.cpp
View File

@ -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();