Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: support Control: Refactor to use Jerk

Browse Source
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 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); 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) 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)) { if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f); 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_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); pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
} else { } else {
// update position controller with new target // update position controller with new target
pos_control->input_accel_xy(guided_accel_target_cmss); 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.x = pos_control->get_vel_desired_cms().x;
guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; 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) { if (!stabilizing_vel_xy() && !do_avoid) {
// set position and velocity errors to zero // set position and velocity errors to zero
pos_control->stop_vel_xy_stabilisation(); pos_control->stop_vel_xy_stabilisation();
@ -695,7 +695,7 @@ void ModeGuided::velaccel_control_run()
// set position errors to zero // set position errors to zero
pos_control->stop_pos_xy_stabilisation(); 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 // call velocity controller which includes z axis controller
pos_control->update_xy_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.x = pos_control->get_pos_target_cm().x;
guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; 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()) { if (!stabilizing_vel_xy()) {
// set position and velocity errors to zero // set position and velocity errors to zero
pos_control->stop_vel_xy_stabilisation(); pos_control->stop_vel_xy_stabilisation();
@ -775,7 +775,7 @@ void ModeGuided::posvelaccel_control_run()
} }
float pz = guided_pos_target_cm.z; 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; guided_pos_target_cm.z = pz;
// run position controllers // run position controllers