From a121fd6b0c469086a0884c835f36cc2228e34b60 Mon Sep 17 00:00:00 2001 From: Sal <32248675+salauddinaliahmed@users.noreply.github.com> Date: Mon, 10 Dec 2018 09:48:58 -0500 Subject: [PATCH] Changed name to attitude rate --- plugins/offboard/offboard_impl.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/plugins/offboard/offboard_impl.cpp b/plugins/offboard/offboard_impl.cpp index 6de3baa5e2..9b5e311228 100644 --- a/plugins/offboard/offboard_impl.cpp +++ b/plugins/offboard/offboard_impl.cpp @@ -160,12 +160,12 @@ void OffboardImpl::set_velocity_body(Offboard::VelocityBodyYawspeed velocity_bod send_velocity_body(); } -void OffboardImpl::set_attitude_target(Offboard::SetAttitude set_attitude) +void OffboardImpl::set_attitude_rate(Offboard::AttitudeRate attitude_rate) { _mutex.lock(); - _set_attitude = set_attitude; + _attitude_rate = attitude_rate; - if (_mode != Mode::ATTITUDE_TARGET) { + if (_mode != Mode::ATTITUDE_RATE) { if (_call_every_cookie) { // If we're already sending other setpoints, stop that now. _parent->remove_call_every(_call_every_cookie); @@ -173,9 +173,9 @@ void OffboardImpl::set_attitude_target(Offboard::SetAttitude set_attitude) } // We automatically send body setpoints from now on. _parent->add_call_every( - [this]() { send_attitude_target(); }, SEND_INTERVAL_S, &_call_every_cookie); + [this]() { send_attitude_rate(); }, SEND_INTERVAL_S, &_call_every_cookie); - _mode = Mode::ATTITUDE_TARGET; + _mode = Mode::ATTITUDE_RATE; } else { // We're already sending these kind of setpoints. Since the setpoint change, let's // reschedule the next call, so we don't send setpoints too often. @@ -184,7 +184,7 @@ void OffboardImpl::set_attitude_target(Offboard::SetAttitude set_attitude) _mutex.unlock(); // also send it right now to reduce latency - send_attitude_target(); + send_attitude_rate(); } void OffboardImpl::send_velocity_ned() @@ -293,7 +293,7 @@ void OffboardImpl::send_velocity_body() _parent->send_message(message); } -void OffboardImpl::send_attitude_target() +void OffboardImpl::send_attitude_rate() { //const static uint8_t IGNORE_BODY_ROLL_RATE = (1 << 0); //const static uint8_t IGNORE_BODY_PITCH_RATE = (1 << 1); @@ -305,10 +305,10 @@ void OffboardImpl::send_attitude_target() const static uint8_t IGNORE_ATTITUDE = (1 << 7); _mutex.lock(); - const float thrust = _set_attitude.thrust_value; - const float body_roll_rate = to_rad_from_deg(_set_attitude.roll_deg_s); - const float body_pitch_rate = to_rad_from_deg(_set_attitude.pitch_deg_s); - const float body_yaw_rate = to_rad_from_deg(_set_attitude.yaw_deg_s); + const float thrust = _attitude_rate.thrust_value; + const float body_roll_rate = to_rad_from_deg(_attitude_rate.roll_deg_s); + const float body_pitch_rate = to_rad_from_deg(_attitude_rate.pitch_deg_s); + const float body_yaw_rate = to_rad_from_deg(_attitude_rate.yaw_deg_s); _mutex.unlock(); mavlink_message_t message;