Skip to content

Commit

Permalink
Copter: Fix rate input frame and clarify SET_ATTITUDE_TARGET
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Jun 20, 2024
1 parent 7579541 commit 5b707f6
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 23 deletions.
26 changes: 14 additions & 12 deletions ArduCopter/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1231,10 +1231,23 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
// this limit is somewhat greater than sqrt(FLT_EPSL)
if (!attitude_quat.is_unit_length()) {
// The attitude quaternion is ill-defined
copter.mode_guided.init(true);
return;
}
}

Vector3f ang_vel_body;
if (!roll_rate_ignore && !pitch_rate_ignore && !yaw_rate_ignore) {
ang_vel_body.x = packet.body_roll_rate;
ang_vel_body.y = packet.body_pitch_rate;
ang_vel_body.z = packet.body_yaw_rate;
} else if (!(roll_rate_ignore && pitch_rate_ignore && yaw_rate_ignore)) {
// The body rates are ill-defined
// input is not valid so stop
copter.mode_guided.init(true);
break;
}

// 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();

Expand All @@ -1256,18 +1269,7 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
}
}

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(attitude_quat, ang_vel,
copter.mode_guided.set_angle(attitude_quat, ang_vel_body,
climb_rate_or_thrust, use_thrust);
}

Expand Down
22 changes: 11 additions & 11 deletions ArduCopter/mode_guided.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ static uint32_t update_time_ms; // system time of last target update
struct {
uint32_t update_time_ms;
Quaternion attitude_quat;
Vector3f ang_vel;
Vector3f ang_vel_body;
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
Expand Down Expand Up @@ -320,7 +320,7 @@ void ModeGuided::angle_control_start()
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero();
guided_angle_state.ang_vel_body.zero();
guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false;
Expand Down Expand Up @@ -638,21 +638,21 @@ bool ModeGuided::use_wpnav_for_position_control() const
}

// 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)
// attitude_quat: IF zero: ang_vel_body (body frame angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and body frame angular velocity
// ang_vel_body: body frame 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)
void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel_body, float climb_rate_cms_or_thrust, bool use_thrust)
{
// check we are in velocity control mode
if (guided_mode != SubMode::Angle) {
angle_control_start();
}

guided_angle_state.attitude_quat = attitude_quat;
guided_angle_state.ang_vel = ang_vel;
guided_angle_state.ang_vel_body = ang_vel_body;

guided_angle_state.use_thrust = use_thrust;
if (use_thrust) {
Expand All @@ -671,7 +671,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_

#if HAL_LOGGING_ENABLED
// log target
copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel_body, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01);
#endif
}

Expand Down Expand Up @@ -946,7 +946,7 @@ void ModeGuided::angle_control_run()
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero();
guided_angle_state.ang_vel_body.zero();
climb_rate_cms = 0.0f;
if (guided_angle_state.use_thrust) {
// initialise vertical velocity controller
Expand Down Expand Up @@ -985,9 +985,9 @@ void ModeGuided::angle_control_run()

// call attitude controller
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);
attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel_body.x) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.y) * 100.0f, ToDeg(guided_angle_state.ang_vel_body.z) * 100.0f);
} else {
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel_body);
}

// call position controller
Expand Down

0 comments on commit 5b707f6

Please sign in to comment.