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

Copter: Support MAVLINK_MSG_ID_SET_ATTITUDE_TARGET

This commit is contained in:
Leonard Hall 2022-01-18 13:01:02 -05:00 committed by Andrew Tridgell
parent 7a9a0bfb3f
commit fd8c180f57
4 changed files with 60 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

@ -401,7 +401,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
Quaternion q;
q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false);
mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
return true;
}

35
ArduCopter/GCS_Mavlink.cpp
View File

@ -1093,11 +1093,24 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
break;
}
// ensure type_mask specifies to use attitude and thrust
if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
const bool roll_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
const bool pitch_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
const bool yaw_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
const bool throttle_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
const bool attitude_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;
// ensure thrust field is not ignored
if (throttle_ignore) {
break;
}
Quaternion attitude_quat;
if (attitude_ignore) {
attitude_quat.zero();
} else {
attitude_quat = Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]);
}
// check if the message's thrust field should be interpreted as a climb rate or as thrust
const bool use_thrust = copter.mode_guided.set_attitude_target_provides_thrust();
@ -1119,15 +1132,19 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
}
}
// 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;
Vector3f ang_vel;
if (!roll_rate_ignore) {
ang_vel.x = packet.body_roll_rate;
}
if (!pitch_rate_ignore) {
ang_vel.y = packet.body_pitch_rate;
}
if (!yaw_rate_ignore) {
ang_vel.z = packet.body_yaw_rate;
}
copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust);
copter.mode_guided.set_angle(attitude_quat, ang_vel,
climb_rate_or_thrust, use_thrust);
break;
}

10
ArduCopter/mode.h
View File

@ -872,7 +872,15 @@ public:
bool requires_terrain_failsafe() const override { return true; }
void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust);
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);
bool 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 terrain_alt = false);
bool set_destination(const Location& 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);
bool get_wp(Location &loc) const override;

61
ArduCopter/mode_guided.cpp
View File

@ -14,9 +14,8 @@ static uint32_t update_time_ms; // system time of last target update
struct {
uint32_t update_time_ms;
float roll_cd;
float pitch_cd;
float yaw_cd;
Quaternion attitude_quat;
Vector3f ang_vel;
float yaw_rate_cds;
float climb_rate_cms; // climb rate in cms. Used if use_thrust is false
float thrust; // thrust from -1 to 1. Used if use_thrust is true
@ -286,9 +285,8 @@ void ModeGuided::angle_control_start()
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.roll_cd = ahrs.roll_sensor;
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
guided_angle_state.attitude_quat.initialise();
guided_angle_state.ang_vel.zero();
guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false;
@ -590,21 +588,22 @@ bool ModeGuided::use_wpnav_for_position_control() const
return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0);
}
// set guided mode angle target and climbrate
void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust)
{
// check we are in velocity control mode
if (guided_mode != SubMode::Angle) {
angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
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.attitude_quat = attitude_quat;
guided_angle_state.ang_vel = ang_vel;
guided_angle_state.use_thrust = use_thrust;
if (use_thrust) {
@ -617,9 +616,13 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust,
guided_angle_state.update_time_ms = millis();
// convert quaternion to euler angles
float roll_rad, pitch_rad, yaw_rad;
attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
// log target
copter.Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(ToDeg(roll_rad) * 100.0f, ToDeg(pitch_rad) * 100.0f, ToDeg(pitch_rad) * 100.0f),
false,
Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f());
}
@ -930,21 +933,6 @@ void ModeGuided::posvelaccel_control_run()
// called from guided_run
void ModeGuided::angle_control_run()
{
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = guided_angle_state.yaw_rate_cds;
float climb_rate_cms = 0.0f;
if (!guided_angle_state.use_thrust) {
// constrain climb rate
@ -957,10 +945,9 @@ void ModeGuided::angle_control_run()
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
roll_in = 0.0f;
pitch_in = 0.0f;
guided_angle_state.attitude_quat.initialise();
guided_angle_state.ang_vel.zero();
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
if (guided_angle_state.use_thrust) {
// initialise vertical velocity controller
pos_control->init_z_controller();
@ -997,10 +984,10 @@ void ModeGuided::angle_control_run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
if (guided_angle_state.attitude_quat.is_zero()) {
attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f);
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
}
// call position controller