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

Copter: implement new set_target methods

Browse Source
This commit is contained in:
Randy Mackay 2021-08-11 20:24:26 +09:00 committed by Andrew Tridgell
parent d17e88aa88
commit 11b72b599e
2 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/Copter.cpp
View File

@ -297,6 +297,19 @@ bool Copter::set_target_location(const Location& target_loc)
return mode_guided.set_destination(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) // set target position and velocity (for use by scripting)
bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) 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()); 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) bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
{ {
// exit if vehicle is not in Guided mode or Auto-Guided mode // 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; 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) 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 // exit if vehicle is not in Guided mode or Auto-Guided mode

3
ArduCopter/Copter.h
View File

@ -645,8 +645,11 @@ private:
#ifdef ENABLE_SCRIPTING #ifdef ENABLE_SCRIPTING
bool start_takeoff(float alt) override; bool start_takeoff(float alt) override;
bool set_target_location(const Location& target_loc) 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_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) 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_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 get_circle_radius(float &radius_m) override;
bool set_circle_rate(float rate_dps) override; bool set_circle_rate(float rate_dps) override;