Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: guided_set_destination accepts relative yaw

This commit is contained in:
Randy Mackay 2017-07-11 14:06:18 +09:00
parent 4c9e118ceb
commit 2b98fd765d
3 changed files with 46 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Copter.h
View File

@ -876,10 +876,10 @@ private:
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0);
bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0);
void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0);
bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
void guided_run();
void guided_takeoff_run();
@ -894,7 +894,7 @@ private:
bool guided_limit_check();
bool guided_nogps_init(bool ignore_checks);
void guided_nogps_run();
void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds);
void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
bool land_init(bool ignore_checks);
void land_run();
void land_gps_run();

36
ArduCopter/GCS_Mavlink.cpp
View File

@ -1611,13 +1611,25 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
}
}
// prepare yaw
float yaw_cd = 0.0f;
bool yaw_relative = false;
float yaw_rate_cds = 0.0f;
if (!yaw_ignore) {
yaw_cd = ToDeg(packet.yaw) * 100.0f;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
}
if (!yaw_rate_ignore) {
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f);
copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_velocity(vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f);
copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!copter.guided_set_destination(pos_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) {
if (!copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_FAILED;
}
} else {
@ -1691,12 +1703,24 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
pos_ned = copter.pv_location_to_vector(loc);
}
// prepare yaw
float yaw_cd = 0.0f;
bool yaw_relative = false;
float yaw_rate_cds = 0.0f;
if (!yaw_ignore) {
yaw_cd = ToDeg(packet.yaw) * 100.0f;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
}
if (!yaw_rate_ignore) {
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f);
copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f);
copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!copter.guided_set_destination(pos_ned, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) {
if (!copter.guided_set_destination(pos_ned, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_FAILED;
}
} else {

21
ArduCopter/control_guided.cpp
View File

@ -177,7 +177,7 @@ void Copter::guided_angle_control_start()
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -195,7 +195,7 @@ bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, f
#endif
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, false);
@ -208,7 +208,7 @@ bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, f
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -233,7 +233,7 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw
}
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
@ -241,7 +241,7 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
@ -249,7 +249,7 @@ void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float y
}
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// record velocity target
guided_vel_target_cms = velocity;
@ -260,14 +260,15 @@ void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float y
}
// set guided mode posvel target
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) {
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
posvel_update_time_ms = millis();
guided_pos_target_cm = destination;
@ -684,10 +685,10 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
}
// helper function to set yaw state and targets
void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
if (use_yaw) {
set_auto_yaw_look_at_heading(yaw_cd, 0.0f, 0, 0);
set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
} else if (use_yaw_rate) {
set_auto_yaw_rate(yaw_rate_cds);
}