Copter: Scripting: add support for `set_target_rate_and_throttle`

This commit is contained in:
Iampete1 2024-09-15 01:02:26 +01:00 committed by Peter Barker
parent 8e6a1a2b02
commit 94944427da
2 changed files with 24 additions and 0 deletions

View File

@ -376,6 +376,7 @@ bool Copter::set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f&
return true;
}
// set target roll pitch and yaw angles with throttle (for use by scripting)
bool Copter::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)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
@ -389,6 +390,27 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
return true;
}
// set target roll pitch and yaw rates with throttle (for use by scripting)
bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
// Zero quaternion indicates rate control
Quaternion q;
q.zero();
// Convert from degrees per second to radians per second
Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
ang_vel_body *= DEG_TO_RAD;
// Pass to guided mode
mode_guided.set_angle(q, ang_vel_body, throttle, true);
return true;
}
#endif
#if MODE_CIRCLE_ENABLED

View File

@ -681,6 +681,8 @@ private:
bool set_target_velocity_NED(const Vector3f& vel_ned) 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;
#endif
#if MODE_CIRCLE_ENABLED
bool get_circle_radius(float &radius_m) override;