diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 2540f0ee15..065fa12314 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -8,7 +8,6 @@ float32 yaw_sp_move_rate # rad/s (commanded by user) # For quaternion-based attitude control float32[4] q_d # Desired quaternion for quaternion control -bool q_d_valid # Set to true if quaternion vector is valid # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index d8a07ea438..ddb6fa4ca6 100644 --- a/src/examples/fixedwing_control/main.cpp +++ b/src/examples/fixedwing_control/main.cpp @@ -208,13 +208,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p } matrix::Eulerf att_spe(roll_body, 0, bearing); - - matrix::Quatf qd(att_spe); - - att_sp->q_d[0] = qd(0); - att_sp->q_d[1] = qd(1); - att_sp->q_d[2] = qd(2); - att_sp->q_d[3] = qd(3); + matrix::Quatf(att_spe).copyTo(att_sp->q_d); } int parameters_init(struct param_handles *handles) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 0ef3ce4073..f7edfafea8 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -169,7 +169,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f7ba1af975..ca02445d49 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1559,7 +1559,6 @@ FixedwingPositionControl::Run() Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; if (_control_mode.flag_control_offboard_enabled || _control_mode.flag_control_position_enabled || diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7b607724dc..0cfd568d6f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3516,17 +3516,7 @@ protected: mavlink_attitude_target_t msg = {}; msg.time_boot_ms = att_sp.timestamp / 1000; - - if (att_sp.q_d_valid) { - memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q)); - - } else { - matrix::Quatf q = matrix::Eulerf(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body); - - for (size_t i = 0; i < 4; i++) { - msg.q[i] = q(i); - } - } + matrix::Quatf(att_sp.q_d).copyTo(msg.q); msg.body_roll_rate = att_rates_sp.roll; msg.body_pitch_rate = att_rates_sp.pitch; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index afd21491f3..89eb06f240 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1491,7 +1491,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (!ignore_attitude_msg) { // only copy att sp if message contained new data matrix::Quatf q(set_attitude_target.q); q.copyTo(att_sp.q_d); - att_sp.q_d_valid = true; matrix::Eulerf euler{q}; att_sp.roll_body = euler.phi(); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 827118156b..7694865d13 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -214,7 +214,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ /* copy quaternion setpoint to attitude setpoint topic */ Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.q_d_valid = true; attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); attitude_setpoint.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 444237a30b..d53d2d0b25 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -94,7 +94,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic Quatf q_sp = R_sp; q_sp.copyTo(att_sp.q_d); - att_sp.q_d_valid = true; // calculate euler angles, for logging only, must not be used for control Eulerf euler = R_sp; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index dacca9b6ce..c994f024e3 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -62,7 +62,6 @@ TEST(PositionControlTest, EmptySetpoint) EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); - //EXPECT_EQ(attitude.q_d_valid, false); // TODO should not be true when there was no control EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); EXPECT_EQ(attitude.roll_reset_integral, false); EXPECT_EQ(attitude.pitch_reset_integral, false); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 9bfdd93659..6a38bcae87 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -91,7 +91,6 @@ GpsFailure::on_active() Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); q.copyTo(att_sp.q_d); - att_sp.q_d_valid = true; if (_navigator->get_vstatus()->is_vtol) { _fw_virtual_att_sp_pub.publish(att_sp); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d5c7200dbe..d434ee6373 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -266,7 +266,6 @@ void Standard::update_transition_state() _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); - _v_att_sp->q_d_valid = true; // check front transition timeout if (_params->front_trans_timeout > FLT_EPSILON) { @@ -282,7 +281,6 @@ void Standard::update_transition_state() _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); - _v_att_sp->q_d_valid = true; _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index fc44ac447b..f9a19881ff 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -261,7 +261,6 @@ void Tailsitter::update_transition_state() _v_att_sp->yaw_body = euler_sp.psi(); _q_trans_sp.copyTo(_v_att_sp->q_d); - _v_att_sp->q_d_valid = true; } void Tailsitter::waiting_on_tecs() diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index b1b0b84b4b..5b30e470c8 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -455,7 +455,6 @@ float VtolType::pusher_assist() const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); - _v_att_sp->q_d_valid = true; } return forward_thrust;