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

Copter: guided mode supports heading and yaw-rate target

Browse Source
This commit is contained in:
kouseii 2017-06-23 00:20:14 +09:00 committed by Randy Mackay
parent 841ce1b631
commit 2e6e429c04
3 changed files with 66 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/Copter.h
View File

@ -856,10 +856,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 guided_set_destination(const Location_Class& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
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);
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();
@ -874,6 +874,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);
bool land_init(bool ignore_checks);
void land_run();
void land_gps_run();

20
ArduCopter/GCS_Mavlink.cpp
View File

@ -1634,12 +1634,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
* bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
// prepare position
@ -1676,11 +1676,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_destination_posvel(pos_vector, vel_vector);
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);
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_velocity(vel_vector);
copter.guided_set_velocity(vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!copter.guided_set_destination(pos_vector)) {
if (!copter.guided_set_destination(pos_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) {
result = MAV_RESULT_FAILED;
}
} else {
@ -1712,12 +1712,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
* bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
Vector3f pos_ned;
@ -1755,11 +1755,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
}
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));
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);
} 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));
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);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!copter.guided_set_destination(pos_ned)) {
if (!copter.guided_set_destination(pos_ned, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) {
result = MAV_RESULT_FAILED;
}
} else {

65
ArduCopter/control_guided.cpp
View File

@ -23,8 +23,9 @@ struct {
float yaw_cd;
float yaw_rate_cds;
float climb_rate_cms;
bool use_yaw;
bool use_yaw_rate;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false};
} static guided_angle_state = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false};
struct Guided_Limit {
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
@ -177,7 +178,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 Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -194,6 +195,9 @@ bool Copter::guided_set_destination(const Vector3f& destination)
}
#endif
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, false);
@ -205,7 +209,7 @@ bool Copter::guided_set_destination(const Vector3f& destination)
// 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 Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -229,19 +233,25 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc)
return false;
}
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity)
void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
// set yaw state
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds);
// record velocity target
guided_vel_target_cms = velocity;
vel_update_time_ms = millis();
@ -251,12 +261,15 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
}
// set guided mode posvel target
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
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) {
// 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);
posvel_update_time_ms = millis();
guided_pos_target_cm = destination;
guided_vel_target_cms = velocity;
@ -426,9 +439,14 @@ void Copter::guided_pos_control_run()
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} else {
if (guided_angle_state.use_yaw_rate) {
// roll & pitch from waypoint controller, yaw rate from GCS
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
}
}
}
@ -480,9 +498,14 @@ void Copter::guided_vel_control_run()
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} else {
if (guided_angle_state.use_yaw_rate) {
// roll & pitch from waypoint controller, yaw rate from GCS
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
}
}
}
@ -554,9 +577,14 @@ void Copter::guided_posvel_control_run()
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
} else {
if (guided_angle_state.use_yaw_rate) {
// roll & pitch from waypoint controller, yaw rate from GCS
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain());
}
}
}
@ -662,6 +690,15 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
pos_control->set_desired_velocity(curr_vel_des);
}
// helper function to set guided yaw state
void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds)
{
guided_angle_state.use_yaw = use_yaw;
guided_angle_state.yaw_cd = yaw_cd;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.yaw_rate_cds = yaw_rate_cds;
}
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits