From 3a05cf2c7a4b3e888319c068a401044305dbc2b2 Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Thu, 3 Feb 2022 01:29:51 -0500 Subject: [PATCH] Copter: add send_attitude_target --- ArduCopter/GCS_Mavlink.cpp | 23 +++++++++++++++++++++++ ArduCopter/GCS_Mavlink.h | 1 + 2 files changed, 24 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6b2009008f..aebade15dc 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -77,6 +77,29 @@ MAV_STATE GCS_MAVLINK_Copter::vehicle_system_status() const } +void GCS_MAVLINK_Copter::send_attitude_target() +{ + const Quaternion quat = copter.attitude_control->get_attitude_target_quat(); + const Vector3f ang_vel = copter.attitude_control->get_attitude_target_ang_vel(); + const float thrust = copter.attitude_control->get_throttle_in(); + + const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4}; + + // Note: When sending out the attitude_target info. we send out all of info. no matter the mavlink typemask + // This way we send out the maximum information that can be used by the sending control systems to adapt their generated trajectories + const uint16_t typemask = 0; // Ignore nothing + + mavlink_msg_attitude_target_send( + chan, + AP_HAL::millis(), // time since boot (ms) + typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle + quat_out, // Attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length + ang_vel.x, // roll rate (rad/s) + ang_vel.y, // pitch rate (rad/s) + ang_vel.z, // yaw rate (rad/s) + thrust); // Collective thrust, normalized to 0 .. 1 +} + void GCS_MAVLINK_Copter::send_position_target_global_int() { Location target; diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index c370682b74..4ee57af953 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -23,6 +23,7 @@ protected: MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; + void send_attitude_target() override; void send_position_target_global_int() override; void send_position_target_local_ned() override;