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

Sub: integrate AC_WPNav::get_speed rename to get_default_speed

This commit is contained in:
Randy Mackay 2019-01-24 14:04:22 +09:00
parent 4d5e13f1d4
commit aa7743e991
5 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/GCS_Mavlink.cpp
View File

@ -904,10 +904,10 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
climb_rate_cms = 0.0f;
} else if (packet.thrust > 0.5f) {
// climb at up to WPNAV_SPEED_UP
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_speed_up();
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up();
} else {
// descend at up to WPNAV_SPEED_DN
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_speed_down());
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_default_speed_down());
}
sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
break;

6
ArduSub/control_auto.cpp
View File

@ -653,7 +653,7 @@ bool Sub::auto_terrain_recover_start()
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
// initialize vertical speeds and leash lengths
pos_control.set_max_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
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
@ -682,12 +682,12 @@ void Sub::auto_terrain_recover_run()
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::RangeFinder_OutOfRangeLow:
target_climb_rate = wp_nav.get_speed_up();
target_climb_rate = wp_nav.get_default_speed_up();
rangefinder_recovery_ms = 0;
break;
case RangeFinder::RangeFinder_OutOfRangeHigh:
target_climb_rate = wp_nav.get_speed_down();
target_climb_rate = wp_nav.get_default_speed_down();
rangefinder_recovery_ms = 0;
break;

4
ArduSub/control_circle.cpp
View File

@ -14,7 +14,7 @@ bool Sub::circle_init()
circle_pilot_yaw_override = false;
// initialize speeds and accelerations
pos_control.set_max_speed_xy(wp_nav.get_speed_xy());
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);
@ -33,7 +33,7 @@ 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_speed_xy());
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);

8
ArduSub/control_guided.cpp
View File

@ -92,7 +92,7 @@ void Sub::guided_posvel_control_start()
pos_control.init_xy_controller();
// set speed and acceleration from wpnav's speed and acceleration
pos_control.set_max_speed_xy(wp_nav.get_speed_xy());
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();
@ -103,7 +103,7 @@ void Sub::guided_posvel_control_start()
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_speed_down(), wp_nav.get_speed_up());
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());
// pilot always controls yaw
@ -117,7 +117,7 @@ void Sub::guided_angle_control_start()
guided_mode = Guided_Angle;
// set vertical speed and acceleration
pos_control.set_max_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
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());
// initialise position and desired velocity
@ -490,7 +490,7 @@ void Sub::guided_angle_control_run()
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_default_speed_down()), wp_nav.get_default_speed_up());
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();

4
ArduSub/control_surface.cpp
View File

@ -8,7 +8,7 @@ bool Sub::surface_init()
}
// initialize vertical speeds and leash lengths
pos_control.set_max_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
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());
// initialise position and desired velocity
@ -48,7 +48,7 @@ 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(abs(wp_nav.get_speed_up()), 1, pos_control.get_max_speed_up());
float cmb_rate = constrain_float(abs(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up());
// record desired climb rate for logging
desired_climb_rate = cmb_rate;