From cf1e0c51827cba510dbc9a80eb568e6bee7a8754 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 1 Aug 2024 22:13:08 +0200 Subject: [PATCH] Remove usage of euler_body angles --- .../FixedwingAttitudeControl.cpp | 29 +-- .../FixedwingPositionControl.cpp | 203 ++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 5 - src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 6 - .../PositionControl/ControlMath.cpp | 6 - .../RoverPositionControl.cpp | 8 +- .../uuv_att_control/uuv_att_control.cpp | 12 +- .../uuv_pos_control/uuv_pos_control.cpp | 5 +- src/modules/vtol_att_control/standard.cpp | 14 +- src/modules/vtol_att_control/tailsitter.cpp | 10 +- src/modules/vtol_att_control/tiltrotor.cpp | 10 +- src/modules/vtol_att_control/vtol_type.cpp | 6 +- 13 files changed, 187 insertions(+), 131 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b08fc895ab9c..cab355695188 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -96,17 +96,16 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) - + radians(_param_fw_psp_off.get()); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, - -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + pitch_body = constrain(pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.reset_integral = false; @@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + Eulerf setpoint(Quatf(_att_sp.q_d)); + float roll_body = setpoint.phi(); + float pitch_body = setpoint.theta(); + + if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + + _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - _wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi()); + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index f636b0e013fd..22958cb399ef 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -451,9 +451,6 @@ FixedwingPositionControl::status_publish() { position_controller_status_s pos_ctrl_status = {}; - pos_ctrl_status.nav_roll = _att_sp.roll_body; - pos_ctrl_status.nav_pitch = _att_sp.pitch_body; - npfg_status_s npfg_status = {}; npfg_status.wind_est_valid = _wind_valid; @@ -791,8 +788,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + Eulerf current_setpoint(Quatf(_att_sp.q_d)); + Quatf setpoint(roll_body, current_setpoint.theta(), yaw_body); + setpoint.copyTo(_att_sp.q_d); } _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; @@ -879,8 +879,12 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: _att_sp.thrust_body[0] = 0.0f; - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = radians(_param_fw_psp_off.get()); + float roll_body = 0.0f; + float pitch_body = radians(_param_fw_psp_off.get()); + float yaw_body = 0.0f; + + Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + setpoint.copyTo(_att_sp.q_d); break; case position_setpoint_s::SETPOINT_TYPE_POSITION: @@ -928,7 +932,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto } /* Copy thrust and pitch values from tecs */ - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); @@ -950,8 +954,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _param_sinkrate_target.get(), _param_climbrate_target.get()); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + float yaw_body = 0.f; if (_landed) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); @@ -960,7 +964,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); } - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } @@ -983,11 +989,13 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) false, descend_rate); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + float yaw_body = 0.f; _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } uint8_t @@ -1109,10 +1117,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1123,6 +1131,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + float pitch_body = get_tecs_pitch(); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1158,10 +1169,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1174,6 +1185,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _param_climbrate_target.get(), false, pos_sp_curr.vz); + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1253,10 +1268,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw float alt_sp = pos_sp_curr.alt; @@ -1267,7 +1282,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - _att_sp.roll_body = 0.0f; + roll_body = 0.0f; } _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); @@ -1282,6 +1297,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } #ifdef CONFIG_FIGURE_OF_EIGHT @@ -1306,7 +1326,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c // Apply control _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - _att_sp.roll_body = _figure_eight.getRollSetpoint(); + float roll_body = _figure_eight.getRollSetpoint(); target_airspeed = _figure_eight.getAirspeedSetpoint(); _target_bearing = _figure_eight.getTargetBearing(); _closest_point_on_path = _figure_eight.getClosestPoint(); @@ -1337,7 +1357,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _param_climbrate_target.get()); // Yaw - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1392,10 +1416,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const 0.0f; navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -1408,7 +1432,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const _param_climbrate_target.get()); _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1494,7 +1521,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1502,7 +1529,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float bearing = _npfg.getBearing(); // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); + float yaw_body = _runway_takeoff.getYaw(bearing); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); @@ -1529,9 +1556,14 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); + float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d) + _flaps_setpoint = _param_fw_flaps_to_scl.get(); // retract ladning gear once passed the climbout state @@ -1590,7 +1622,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1615,17 +1647,27 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _att_sp.thrust_body[0] = get_tecs_thrust(); } - _att_sp.pitch_body = get_tecs_pitch(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float pitch_body = get_tecs_pitch(); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d) } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; + float roll_body = 0.0f; + float yaw_body = _yaw; _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); + float pitch_body = radians(_takeoff_pitch_min.get()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d) + } launch_detection_status_s launch_detection_status; @@ -1634,8 +1676,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_detection_status_pub.publish(launch_detection_status); } - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt); - if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); } @@ -1739,10 +1779,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + // _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); // use npfg's bearing to commanded course, controlled via yaw angle while on runway - _att_sp.yaw_body = _npfg.getBearing(); + // _att_sp.yaw_body = _npfg.getBearing(); /* longitudinal guidance */ @@ -1788,7 +1828,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + // _att_sp.pitch_body = get_tecs_pitch(); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -1818,7 +1858,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + // _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -1840,10 +1880,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + // _att_sp.pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + // _att_sp.yaw_body = _yaw; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -1853,7 +1893,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + // _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1912,6 +1952,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } // the terrain estimate (if enabled) is always used to determine the flaring altitude + float roll_body; + float yaw_body; + float pitch_body; + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -1944,9 +1988,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw /* longitudinal guidance */ @@ -1991,7 +2035,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -2021,7 +2065,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -2045,10 +2089,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + yaw_body = _yaw; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -2058,7 +2102,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2105,11 +2153,14 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, false, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -2138,6 +2189,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _time_last_xy_reset = _local_pos.timestamp; } + Eulerf current_setpoint(Quatf(_att_sp.q_d)); + float yaw_body = current_setpoint.psi(); + float roll_body = current_setpoint.phi(); + float pitch_body = current_setpoint.theta(); + /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2181,10 +2237,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } } @@ -2206,12 +2262,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + + pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, @@ -2234,10 +2294,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -2250,7 +2310,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _param_climbrate_target.get()); _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } float FixedwingPositionControl::get_tecs_pitch() @@ -2562,10 +2625,10 @@ FixedwingPositionControl::Run() if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()), - radians(_param_fw_r_lim.get())); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get())); + roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()), + radians(_param_fw_r_lim.get())); + pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get())); } if (_control_mode.flag_control_position_enabled || @@ -2575,9 +2638,9 @@ FixedwingPositionControl::Run() _control_mode.flag_control_climb_rate_enabled) { // roll slew rate - _att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval); + roll_body = _roll_slew_rate.update(roll_body, control_interval); - const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); @@ -3166,8 +3229,10 @@ float FixedwingPositionControl::getLoadFactor() { float load_factor_from_bank_angle = 1.0f; - if (PX4_ISFINITE(_att_sp.roll_body)) { - load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON); + float roll_body = Eulerf(Quatf(_att_sp.q_d)); + + if (PX4_ISFINITE(roll_body)) { + load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON); } return load_factor_from_bank_angle; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..5f243492857c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1598,11 +1598,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) const matrix::Quatf q{attitude_target.q}; q.copyTo(attitude_setpoint.q_d); - matrix::Eulerf euler{q}; - attitude_setpoint.roll_body = euler.phi(); - attitude_setpoint.pitch_body = euler.theta(); - attitude_setpoint.yaw_body = euler.psi(); - // TODO: review use case attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : attitude_target.body_yaw_rate; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 32d244b138da..6aca9f7d9837 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -264,7 +264,9 @@ class MavlinkStreamHighLatency2 : public MavlinkStream vehicle_attitude_setpoint_s attitude_sp; if (_attitude_sp_sub.update(&attitude_sp)) { - msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + + msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf( + attitude_sp.q_d)).psi())) * 0.5f); return true; } 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 39cb033e74dc..f6cc5db43d88 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, q_sp.copyTo(attitude_setpoint.q_d); - // Transform to euler angles for logging only - const Eulerf euler_sp(q_sp); - attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = euler_sp(1); - attitude_setpoint.yaw_body = euler_sp(2); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); 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 b4eba96115cd..3d1b1268c2cc 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d); - - // calculate euler angles, for logging only, must not be used for control - const Eulerf euler{R_sp}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..b691438b337c 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll() if (_control_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = 0.0; - _att_sp.pitch_body = 0.0; + float roll_body = 0.0; + float pitch_body = 0.0; /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { @@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll() _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); } - _att_sp.yaw_body = _manual_yaw_sp; + float yaw_body = _manual_yaw_sp; _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 58992ccb7803..adbcaee2db3c 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude */ Eulerf euler_angles(matrix::Quatf(attitude.q)); - float roll_body = attitude_setpoint.roll_body; - float pitch_body = attitude_setpoint.pitch_body; - float yaw_body = attitude_setpoint.yaw_body; + Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d)); + float roll_body = setpoint_euler_angles(0); + float pitch_body = setpoint_euler_angles(1); + float yaw_body = setpoint_euler_angles(2); float roll_rate_desired = rates_setpoint.roll; float pitch_rate_desired = rates_setpoint.pitch; @@ -227,9 +228,8 @@ void UUVAttitudeControl::Run() _vehicle_rates_setpoint_sub.update(&_rates_setpoint); if (input_mode == 1) { // process manual data - _attitude_setpoint.roll_body = _param_direct_roll.get(); - _attitude_setpoint.pitch_body = _param_direct_pitch.get(); - _attitude_setpoint.yaw_body = _param_direct_yaw.get(); + Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get())); + attitude_setpoint.copyTo(_attitude_setpoint.q_d); _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); _attitude_setpoint.thrust_body[1] = 0.f; _attitude_setpoint.thrust_body[2] = 0.f; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index d402941b4a16..9d164b88f71a 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); - vehicle_attitude_setpoint.roll_body = roll_des; - vehicle_attitude_setpoint.pitch_body = pitch_des; - vehicle_attitude_setpoint.yaw_body = yaw_des; + Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des)); + attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d); vehicle_attitude_setpoint.thrust_body[0] = thrust_x; vehicle_attitude_setpoint.thrust_body[1] = thrust_y; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 84606aad4327..868041db314b 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -181,7 +181,6 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -227,21 +226,22 @@ void Standard::update_transition_state() } // ramp up FW_PSP_OFF - _v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); - + const Eulerf setpoint_euler(Quatf(_v_att_sp->q_d)); + const float pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); _v_att_sp->thrust_body[0] = _pusher_throttle; - - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(setpoint_euler.phi(), pitch_body, setpoint_euler.psi())); q_sp.copyTo(_v_att_sp->q_d); } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { + Eulerf setpoint_euler(Quatf(_v_att_sp->q_d)); + float pitch_body = setpoint_euler.theta(); if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(setpoint_euler.phi(), pitch_body, setpoint_euler.psi())); q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index af95a05083c3..4254c9613140 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -179,7 +179,8 @@ void Tailsitter::update_transition_state() // the yaw setpoint and zero roll since we want wings level transition. // If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch if (_fw_virtual_att_sp->timestamp > (now - 1_s)) { - _q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp); + float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta(); + _q_trans_start = Eulerf(0.f, pitch_body, yaw_sp); } else { _q_trans_start = Eulerf(0.f, 0.f, yaw_sp); @@ -191,7 +192,8 @@ void Tailsitter::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level - _q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d)); + _q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi()); Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f); _trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f)); } @@ -238,10 +240,6 @@ void Tailsitter::update_transition_state() _v_att_sp->timestamp = hrt_absolute_time(); const Eulerf euler_sp(_q_trans_sp); - _v_att_sp->roll_body = euler_sp.phi(); - _v_att_sp->pitch_body = euler_sp.theta(); - _v_att_sp->yaw_body = euler_sp.psi(); - _q_trans_sp.copyTo(_v_att_sp->q_d); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 0956e1003698..0610c347f212 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state() const hrt_abstime now = hrt_absolute_time(); + Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -212,7 +217,6 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -290,7 +294,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { @@ -321,7 +325,7 @@ void Tiltrotor::update_transition_state() _v_att_sp->thrust_body[2] = -_thrust_transition; - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index e43dd7c912ad..458b3927fb9c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -563,10 +563,10 @@ float VtolType::pusher_assist() tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints - _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); - _v_att_sp->roll_body = -asinf(tilt_new(1)); + const float pitch_body = atan2f(tilt_new(0), tilt_new(2)); + const float roll_body = -asinf(tilt_new(1)); - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); + const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); }