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

Copter: Fix rate input frame and clarify SET_ATTITUDE_TARGET

This commit is contained in:
Leonard Hall 2024-06-25 08:55:23 +09:30 committed by Peter Barker
parent 7047e11090
commit b45b07d539
2 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/GCS_Mavlink.cpp
View File

@ -1217,6 +1217,7 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
// ensure thrust field is not ignored
if (throttle_ignore) {
// The throttle input is not defined
return;
}
@ -1235,6 +1236,18 @@ void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_messag
}
}
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);
return;
}
// 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();
@ -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);
}

22
ArduCopter/mode_guided.cpp
View File

@ -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
@ -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;
@ -638,13 +638,13 @@ 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) {
@ -652,7 +652,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
}
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) {
@ -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
}
@ -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
@ -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