fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove euler angles from attitude setpoint (#23482)

Browse Source 
* Remove euler angles from attitude setpoint message

* Remove usage of euler angles in attitude setpoint messages

This commit removes the usage of euler angles in the vehicle_attitude_setpoint messages

* Fix standard vtol
This commit is contained in:
Jaeyoung Lim 2024-08-12 16:42:51 +02:00 committed by GitHub
parent c9343ca11d
commit e008ca24f1
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
16 changed files with 234 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/VehicleAttitudeSetpoint.msg
View File

@ -1,9 +1,5 @@
uint64 timestamp # time since system start (microseconds)
float32 roll_body # body angle in NED frame (can be NaN for FW)
float32 pitch_body # body angle in NED frame (can be NaN for FW)
float32 yaw_body # body angle in NED frame (can be NaN for FW)
float32 yaw_sp_move_rate # rad/s (commanded by user)
# For quaternion-based attitude control

29
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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));
const 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(),
const Eulerf setpoint(Quatf(_att_sp.q_d));
const float roll_body = setpoint.phi();
const 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();

227
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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
const Eulerf current_setpoint(Quatf(_att_sp.q_d));
const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body));
setpoint.copyTo(_att_sp.q_d);
}
_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
@ -877,11 +877,16 @@ 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());
break;
case position_setpoint_s::SETPOINT_TYPE_IDLE: {
_att_sp.thrust_body[0] = 0.0f;
const float roll_body = 0.0f;
const float pitch_body = radians(_param_fw_psp_off.get());
const float yaw_body = 0.0f;
const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body));
setpoint.copyTo(_att_sp.q_d);
break;
}
case position_setpoint_s::SETPOINT_TYPE_POSITION:
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
@ -927,9 +932,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
}
/* Copy thrust and pitch values from tecs */
_att_sp.pitch_body = get_tecs_pitch();
if (!_vehicle_status.in_transition_to_fw) {
publishLocalPositionSetpoint(current_sp);
}
@ -950,8 +952,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;
const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
const float yaw_body = 0.f;
if (_landed) {
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
@ -960,7 +962,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();
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
@ -983,11 +987,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;
const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
const 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();
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
uint8_t
@ -1109,10 +1115,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 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
void
@ -1158,10 +1167,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 +1183,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_param_climbrate_target.get(),
false,
pos_sp_curr.vz);
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
void
@ -1253,10 +1266,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 +1280,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 +1295,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
#ifdef CONFIG_FIGURE_OF_EIGHT
@ -1306,7 +1324,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 +1355,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
const float pitch_body = get_tecs_pitch();
const 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 +1414,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 +1430,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();
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
void
@ -1494,7 +1519,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 +1527,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 +1554,16 @@ 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());
const 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);
const 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
if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) {
_new_landing_gear_position = landing_gear_s::GEAR_UP;
@ -1588,7 +1620,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;
@ -1613,17 +1645,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;
@ -1633,7 +1675,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
}
_flaps_setpoint = _param_fw_flaps_to_scl.get();
_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);
@ -1738,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();
float roll_body = getCorrectedNpfgRollSetpoint();
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
_att_sp.yaw_body = _npfg.getBearing();
float yaw_body = _npfg.getBearing();
/* longitudinal guidance */
@ -1787,7 +1828,13 @@ 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();
float pitch_body = get_tecs_pitch();
roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
// enable direct yaw control using rudder/wheel
_att_sp.fw_control_yaw_wheel = true;
@ -1817,7 +1864,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();
float roll_body = getCorrectedNpfgRollSetpoint();
/* longitudinal guidance */
@ -1839,10 +1886,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
/* set the attitude and throttle commands */
_att_sp.pitch_body = get_tecs_pitch();
float pitch_body = get_tecs_pitch();
roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
// yaw is not controlled in nominal flight
_att_sp.yaw_body = _yaw;
float yaw_body = _yaw;
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
// enable direct yaw control using rudder/wheel
_att_sp.fw_control_yaw_wheel = false;
@ -1852,8 +1904,6 @@ 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);
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
@ -1911,6 +1961,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
@ -1943,9 +1997,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 */
@ -1990,7 +2044,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;
@ -2020,7 +2074,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 */
@ -2044,10 +2098,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;
@ -2057,7 +2111,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();
@ -2104,11 +2162,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();
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
void
@ -2137,6 +2198,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 &&
@ -2177,10 +2243,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
navigateLine(_hdg_hold_position, _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
}
}
@ -2202,12 +2268,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,
@ -2230,10 +2300,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,
@ -2246,7 +2316,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();
const float pitch_body = get_tecs_pitch();
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
attitude_setpoint.copyTo(_att_sp.q_d);
}
float
FixedwingPositionControl::get_tecs_pitch()
@ -2556,12 +2629,16 @@ FixedwingPositionControl::Run()
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
float roll_body = attitude_setpoint.phi();
float pitch_body = attitude_setpoint.theta();
float yaw_body = attitude_setpoint.psi();
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(roll_body, -radians(_param_fw_r_lim.get()),
radians(_param_fw_r_lim.get()));
pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()));
}
if (_control_mode.flag_control_position_enabled ||
@ -2571,9 +2648,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();
@ -3162,8 +3239,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)).phi();
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;

5
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1626,11 +1626,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;

4
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -264,7 +264,9 @@ private:
vehicle_attitude_setpoint_s attitude_sp;
if (_attitude_sp_sub.update(&attitude_sp)) {
msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf(
attitude_sp.q_d)).psi())) * 0.5f);
return true;
}

6
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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();

6
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -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)

21
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
View File

@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
float yaw = 0.f;
vehicle_attitude_setpoint_s att{};
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
EXPECT_FLOAT_EQ(att.q_d[0], 1.f);
EXPECT_FLOAT_EQ(att.q_d[1], 0.f);
EXPECT_FLOAT_EQ(att.q_d[2], 0.f);
EXPECT_FLOAT_EQ(att.q_d[3], 0.f);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
/* expected: same as before but with 90 yaw
* reason: only yaw changed */
yaw = M_PI_2_F;
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
Eulerf euler_att(Quatf(att.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
/* expected: same as before but roll 180
@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
* order is: 1. roll, 2. pitch, 3. yaw */
thr = Vector3f(0.f, 0.f, 1.f);
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
Eulerf euler_att2(Quatf(att.q_d));
EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F));
EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
}

20
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
View File

@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint)
vehicle_attitude_setpoint_s attitude{};
position_control.getAttitudeSetpoint(attitude);
EXPECT_FLOAT_EQ(attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f);
EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f);
Eulerf euler_att(Quatf(attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
EXPECT_FLOAT_EQ(euler_att.psi(), 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(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);
// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
}
@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), -1.f);
}
TEST_F(PositionControlBasicTest, FailsafeInput)
@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
EXPECT_TRUE(runController());
// THEN: the integral did not wind up and produce unexpected deviation
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
Eulerf euler_att(Quatf(_attitude.q_d));
EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
}

8
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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));
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.timestamp = hrt_absolute_time();

12
src/modules/uuv_att_control/uuv_att_control.cpp
View File

@ -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;
const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d));
const float roll_body = setpoint_euler_angles(0);
const float pitch_body = setpoint_euler_angles(1);
const 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;

5
src/modules/uuv_pos_control/uuv_pos_control.cpp
View File

@ -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;
const 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;

17
src/modules/vtol_att_control/standard.cpp
View File

@ -172,6 +172,11 @@ void Standard::update_transition_state()
VtolType::update_transition_state();
const 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 climbrate 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) {
@ -181,7 +186,7 @@ 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;
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
} else {
// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
@ -227,21 +232,19 @@ 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);
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(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
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(roll_body, pitch_body, yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_pusher_throttle = 0.0f;

10
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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);
const 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);
const 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);
}

11
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state()
const hrt_abstime now = hrt_absolute_time();
const 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,7 @@ 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;
roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
} else {
@ -290,7 +295,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 +326,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);

6
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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);
}