Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: allow copter to yaw to velocity target

Browse Source 
add set_target_angle_and_rate_and_throttle() for precise vehicle control
don't allow set_target_angle_and_rate_and_throttle() to be called unless in guided mode.
This commit is contained in:
Andy Piper
2025-12-17 16:31:37 +00:00
parent 9562c98f84
commit 36e6aef981
2 changed files with 32 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/Copter.cpp
+30 -2
View File
@@ -341,14 +341,23 @@ bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const
return mode_guided.set_pos_vel_accel_NED_m(target_pos_ned_m.topostype(), target_vel_ned_ms, target_accel_ned_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative);
}
bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms)
bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms, bool align_yaw_to_target)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
mode_guided.set_vel_NED_ms(target_vel_ned_ms);
// optionally line up the copter with the velocity vector
float yaw_rads = 0.0f;
if (align_yaw_to_target) {
const float speed_sq = target_vel_ned_ms.xy().length_squared();
if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS))) {
yaw_rads = atan2f(target_vel_ned_ms.y, target_vel_ned_ms.x);
}
}
mode_guided.set_vel_accel_NED_m(target_vel_ned_ms, Vector3f(), align_yaw_to_target, yaw_rads);
return true;
}
@@ -400,6 +409,25 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
return true;
}
// set target roll pitch and yaw angles and roll pitch and yaw rates with throttle (for use by scripting)
bool Copter::set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
Quaternion q;
q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
// Convert from degrees per second to radians per second
Vector3f ang_vel_body_degs { roll_rate_degs, pitch_rate_degs, yaw_rate_degs };
ang_vel_body_degs *= DEG_TO_RAD;
mode_guided.set_angle(q, ang_vel_body_degs, throttle, true);
return true;
}
// Register a custom mode with given number and names
AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
{
ArduCopter/Copter.h
+2 -1
View File
@@ -684,10 +684,11 @@ private:
bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt) override;
bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
bool set_target_velocity_NED(const Vector3f& vel_ned) override;
bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) override;
bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle) override;
// Register a custom mode with given number and names
AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;