From a7db178753ebbc74a02ac85fcf7b0857e27d2ac5 Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Sun, 7 Apr 2024 22:39:01 +0530 Subject: [PATCH] addition for handling angular goals (not compiling) --- ArduCopter/AP_ExternalControl_Copter.cpp | 12 +++++++ ArduCopter/AP_ExternalControl_Copter.h | 7 ++++ ArduCopter/Copter.cpp | 2 +- ArduCopter/GCS_Mavlink.cpp | 3 +- ArduCopter/mode.h | 2 +- ArduCopter/mode_guided.cpp | 7 ++-- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 5 ++- .../AC_AttitudeControl/AC_AttitudeControl.h | 7 +++- .../AC_AttitudeControl_Multi_6DoF.cpp | 4 +-- .../AC_AttitudeControl_Multi_6DoF.h | 2 +- libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 33 ++++++++++++++----- .../ardupilot_msgs/msg/AngularVelandAccn.idl | 2 +- .../AP_ExternalControl/AP_ExternalControl.h | 8 +++++ 13 files changed, 72 insertions(+), 22 deletions(-) diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp index 35353a412618f6..94f740e97e42a1 100644 --- a/ArduCopter/AP_ExternalControl_Copter.cpp +++ b/ArduCopter/AP_ExternalControl_Copter.cpp @@ -29,6 +29,18 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f return true; } +/* + Sets the target orientation, angular velocity, angular acceleration and desired thrust + in body frame +*/ +bool set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust) +{ + if (!ready_for_external_control()) { + return false + } + copter.mode_guided.set_angle(orientation, angular_velocity, angular_acceleration, thrust, true) +} + bool AP_ExternalControl_Copter::ready_for_external_control() { return copter.flightmode->in_guided_mode() && copter.motors->armed(); diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h index e7601c5552fc6c..8a8a799b5abf5f 100644 --- a/ArduCopter/AP_ExternalControl_Copter.h +++ b/ArduCopter/AP_ExternalControl_Copter.h @@ -15,6 +15,13 @@ class AP_ExternalControl_Copter : public AP_ExternalControl { Yaw is in earth frame, NED [rad/s]. */ bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override WARN_IF_UNUSED; + + /* + Sets the target orientation, angular velocity, angular acceleration and desired thrust + in body frame + */ + bool set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust) override WARN_IF_UNUSED; + private: /* Return true if Copter is ready to handle external control data. diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0460de12ecf821..ab45ddddb72a46 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -382,7 +382,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, Vector3f{}, climb_rate_ms*100, false); + mode_guided.set_angle(q, Vector3f{}, Vector3f{},climb_rate_ms*100, false); return true; } #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9c7eeccf1644e2..1cd13be62e4d83 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1268,7 +1268,8 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) ang_vel.z = packet.body_yaw_rate; } - copter.mode_guided.set_angle(attitude_quat, ang_vel, + Vector3f angular_acceleration{0.0, 0.0, 0.0}; // only for use with custom controllers, hence 0 by default + copter.mode_guided.set_angle(attitude_quat, ang_vel, angular_acceleration, climb_rate_or_thrust, use_thrust); break; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 2e33f121c3026c..a92f21e093781e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1037,7 +1037,7 @@ class ModeGuided : public Mode { // 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); + void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, const Vector3f &ang_acceleration ,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); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index e52bfdae310128..9695ee8c5341c5 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -16,6 +16,7 @@ struct { uint32_t update_time_ms; Quaternion attitude_quat; Vector3f ang_vel; + Vector3f ang_acceleration; // angular acceleration, not used by main PID controller but can be used in custom controllers 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 @@ -315,6 +316,7 @@ void ModeGuided::angle_control_start() 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_acceleration.zero(); guided_angle_state.climb_rate_cms = 0.0f; guided_angle_state.yaw_rate_cds = 0.0f; guided_angle_state.use_yaw_rate = false; @@ -635,7 +637,7 @@ bool ModeGuided::use_wpnav_for_position_control() const // 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, const Vector3f &ang_acceleration,float climb_rate_cms_or_thrust, bool use_thrust) { // check we are in velocity control mode if (guided_mode != SubMode::Angle) { @@ -644,6 +646,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_acceleration = ang_acceleration; guided_angle_state.use_thrust = use_thrust; if (use_thrust) { @@ -978,7 +981,7 @@ void ModeGuided::angle_control_run() 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_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, guided_angle_state.ang_acceleration); } // call position controller diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c68e143a112fb1..3bd7c3236352d7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -228,12 +228,11 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step by the integral of the angular velocity -void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) +void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target) { Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; Vector3f attitude_error_angle; attitude_error_quat.to_axis_angle(attitude_error_angle); - // Limit the angular velocity ang_vel_limit(ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); @@ -260,7 +259,7 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec attitude_desired_update.from_axis_angle(ang_vel_target * _dt); attitude_desired_quat = attitude_desired_quat * attitude_desired_update; attitude_desired_quat.normalize(); - + _ang_acceleration_target = ang_acceleration_target; // Call quaternion attitude controller attitude_controller_run_quat(); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 29ec8eae60682a..9f16c9a448ecab 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -161,7 +161,8 @@ class AC_AttitudeControl { // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity - virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target); + // Angular acceleration target will be ignored, only for use with custom controllers + virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target); // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); @@ -491,6 +492,10 @@ class AC_AttitudeControl { // It is reset to zero immediately after it is used. Vector3f _sysid_ang_vel_body; + // This is the angular acceleration in radians per second^2 in the body frame. This is not used in + // main attitude controller but can be used in custom attitude controllers. + Vector3f _ang_acceleration_target; + // This is the unitless value added to the output of the PID by the System Identification Mode. // It is reset to zero immediately after it is used. Vector3f _actuator_sysid; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp index c1fe903b698414..83a5835f15eb2b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp @@ -149,7 +149,7 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity // not used anywhere in current code, panic in SITL so this implementation is not overlooked -void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) { +void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF"); #endif @@ -157,7 +157,7 @@ void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desire _motors.set_lateral(0.0f); _motors.set_forward(0.0f); - AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target); + AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target, ang_acceleration_target); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index 8023985eabb969..4cd73a402c7267 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -21,7 +21,7 @@ class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi { // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity // not used anywhere in current code, panic so this implementation is not overlooked - void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override; + void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target) override; /* override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles */ diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index d0080af023b27f..be294bef0233fa 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -92,15 +92,30 @@ bool AP_DDS_External_Control::handle_angular_control(custom_msgs_msg_AngularVela return false; } - // Convert commands from body frame (x-forward, y-left, z-up) to NED. - //Quaternion orientation; - //Vector3f angular_velocity; - //Vector3f angular_acceleration; - //double thrust; - - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "angular control"); - - return true; + if(strcmp(cmd_angular_goals.header.frame_id, BASE_LINK_FRAME_ID) == 0) + { + // commands in body frame + Quaternion orientation; + orientation[0] = cmd_angular_goals.orientation.x; + orientation[1] = cmd_angular_goals.orientation.y; + orientation[2] = cmd_angular_goals.orientation.z; + orientation[3] = cmd_angular_goals.orientation.w; + + Vector3f angular_velocity; + angular_velocity[0] = cmd_angular_goals.angular_rate.x; + angular_velocity[1] = cmd_angular_goals.angular_rate.y; + angular_velocity[2] = cmd_angular_goals.angular_rate.z; + + Vector3f angular_acceleration; + angular_acceleration[0] = cmd_angular_goals.angular_acceleration.x; + angular_acceleration[1] = cmd_angular_goals.angular_acceleration.y; + angular_acceleration[2] = cmd_angular_goals.angular_acceleration.z; + + float thrust = cmd_angular_goals.thrust; + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "angular control"); + return external_control->set_angular_goals(orientation, angular_velocity, angular_acceleration, thrust); + } + return false; } bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out) diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl index c76e6329e1df12..08614e0649304c 100644 --- a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl @@ -28,7 +28,7 @@ module custom_msgs { @verbatim (language="comment", text="Thrust represented as a single floating-point number. \ It describes the magnitude of the thrust force.") - double thrust; + floatgit thrust; }; }; }; diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index 34228e2b7ab4ff..876a86f6a67197 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -25,6 +25,14 @@ class AP_ExternalControl return false; } + /* + Sets the target orientation, angular velocity, angular acceleration and desired thrust + in body frame + */ + virtual bool set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust) WARN_IF_UNUSED { + return true; + } + /* Sets the target global position with standard guided mode behavior. */