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

Copter: support Control: Refactor to use Jerk

This commit is contained in:
Leonard Hall 2021-08-06 19:11:12 +09:30 committed by Andrew Tridgell
parent 62c4a34660
commit 0d71950e41
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

@ -307,7 +307,7 @@ bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& t
const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
return mode_guided.set_destination_posvel(pos_neu_cm, vel_neu_cms);
return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f());
}
bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)

12
ArduCopter/mode_guided.cpp
View File

@ -608,8 +608,8 @@ void ModeGuided::accel_control_run()
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
}
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy());
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
} else {
// update position controller with new target
pos_control->input_accel_xy(guided_accel_target_cmss);
@ -687,7 +687,7 @@ void ModeGuided::velaccel_control_run()
guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
}
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy());
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
if (!stabilizing_vel_xy() && !do_avoid) {
// set position and velocity errors to zero
pos_control->stop_vel_xy_stabilisation();
@ -695,7 +695,7 @@ void ModeGuided::velaccel_control_run()
// set position errors to zero
pos_control->stop_pos_xy_stabilisation();
}
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
// call velocity controller which includes z axis controller
pos_control->update_xy_controller();
@ -760,7 +760,7 @@ void ModeGuided::posvelaccel_control_run()
guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
}
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy());
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
if (!stabilizing_vel_xy()) {
// set position and velocity errors to zero
pos_control->stop_vel_xy_stabilisation();
@ -775,7 +775,7 @@ void ModeGuided::posvelaccel_control_run()
}
float pz = guided_pos_target_cm.z;
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z);
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
guided_pos_target_cm.z = pz;
// run position controllers