diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index afea2fdef9..35a6688a55 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -297,6 +297,19 @@ bool Copter::set_target_location(const Location& target_loc) return mode_guided.set_destination(target_loc); } +// set target position (for use by scripting) +bool Copter::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 terrain_alt) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f); + + return mode_guided.set_destination(pos_neu_cm, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative, terrain_alt); +} + // set target position and velocity (for use by scripting) bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { @@ -311,6 +324,21 @@ bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& t return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f()); } +// set target position, velocity and acceleration (for use by scripting) +bool Copter::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) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + 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 accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f); + + return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, accel_neu_cms, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative); +} + bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) { // exit if vehicle is not in Guided mode or Auto-Guided mode @@ -324,6 +352,22 @@ bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) return true; } +// set target velocity and acceleration (for use by scripting) +bool Copter::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) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + // convert vector to neu in cm + const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f); + const Vector3f accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f); + + mode_guided.set_velaccel(vel_neu_cms, accel_neu_cms, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, relative_yaw); + return true; +} + 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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0255866a22..d9f6f441d5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -648,8 +648,11 @@ private: #ifdef ENABLE_SCRIPTING bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; + 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 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_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 get_circle_radius(float &radius_m) override; bool set_circle_rate(float rate_dps) override;