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

Copter: accept yaw rate commands in Guided atttude controller

This commit is contained in:
Allan Matthew 2016-08-02 10:20:57 -07:00 committed by Randy Mackay
parent 270fa95a8d
commit 6dd4e1a2aa
3 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -810,7 +810,7 @@ private:
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);
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
void guided_set_angle(const Quaternion &q, float yaw_rate_rads, bool use_yaw_rate, float climb_rate_cms);
void guided_run();
void guided_takeoff_run();
void guided_pos_control_run();

12
ArduCopter/GCS_Mavlink.cpp
View File

@ -1685,7 +1685,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// descend at up to WPNAV_SPEED_DN
climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav.get_speed_down());
}
copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
// if the body_yaw_rate field is ignored, use the commanded yaw position
// otherwise use the commanded yaw rate
bool use_yaw_rate = false;
if ((packet.type_mask & (1<<2)) == 0) {
use_yaw_rate = true;
}
copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
packet.body_yaw_rate, use_yaw_rate, climb_rate_cms);
break;
}

18
ArduCopter/control_guided.cpp
View File

@ -23,8 +23,10 @@ struct {
float roll_cd;
float pitch_cd;
float yaw_cd;
float yaw_rate_cds;
float climb_rate_cms;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};
bool use_yaw_rate;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false};
struct Guided_Limit {
uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
@ -163,6 +165,8 @@ void Copter::guided_angle_control_start()
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -262,7 +266,7 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
void Copter::guided_set_angle(const Quaternion &q, float yaw_rate_rads, bool use_yaw_rate, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
@ -274,6 +278,8 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
@ -569,6 +575,7 @@ void Copter::guided_angle_control_run()
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
@ -579,13 +586,18 @@ void Copter::guided_angle_control_run()
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
if (guided_angle_state.use_yaw_rate) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
} else {
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
}
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);