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

Omni Pos-Ctrl: Added a parameter for the attitude change rate (OMNI_ATT_MODE=6)

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-07-28 21:30:23 -04:00
parent 948e75c230
commit 431a0cb11e
6 changed files with 57 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,8 @@ namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode,
const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, int omni_proj_axes,
vehicle_attitude_setpoint_s &att_sp)
{
// Print an error if the omni_att_mode parameter is out of range
@ -76,7 +77,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
}
case 6: { // Attitude is changed very slowly given the rate
float tilt_rate = 0.01;
float tilt_rate = math::radians(omni_att_rate);
thrustToSlowAttitude(thr_sp, yaw_sp, att, tilt_rate, omni_proj_axes, att_sp);
break;
}
@ -88,17 +89,23 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
// Estimate the optimal tilt angle and direction to counteract the wind
if (omni_att_mode == 5 || omni_att_mode == 6) {
// Calculate the tilt angle
omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());
// Calculate the tilt direction
omni_att_tilt_dir = wrap_2pi(atan2f(thr_sp(1), thr_sp(0)));
// Calculate the current z axis
Vector3f curr_z;
matrix::Dcmf R_body = att;
// Set the roll angle
omni_att_roll = att_sp.roll_body;
for (int i = 0; i < 3; i++) {
curr_z(i) = R_body(i, 2);
}
// Set the pitch angle
omni_att_pitch = att_sp.pitch_body;
// Calculate the tilt angle and direction
omni_att_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
omni_att_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0)));
// Calculate the roll and pitch
Eulerf euler = R_body;
omni_att_roll = euler(0);
omni_att_pitch = euler(1);
}
}

18
src/modules/mc_pos_control/PositionControl/ControlMath.hpp
View File

@ -50,17 +50,21 @@ namespace ControlMath
* @param thr_sp desired 3D thrust vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
* @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt
* @param omni_att_mode attitude mode for omnidirectional vehicles
* @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
* @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees)
* @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees)
* @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
* @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees)
* @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
* @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
* @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
* @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians)
* @param omni_att_rate the attitude change rate for mode=6
* @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
* @param att_sp attitude setpoint to fill
*/
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
vehicle_attitude_setpoint_s &att_sp);
/**
* Converts a body z vector and yaw set-point to a desired attitude.
* @param body_z a world frame 3D vector in direction of the desired body z axis
@ -122,7 +126,7 @@ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp,
* @param thr_sp a 3D vector
* @param yaw_sp the desired yaw
* @param att current attitude of the robot
* @param tilt_rate rate for the tilt change (0-1)
* @param tilt_rate rate for the tilt change (non-negative in radians per loop)
* @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
* @param att_sp attitude setpoint to fill
*/

5
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -363,9 +363,10 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode,
const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll,
float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const
float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
vehicle_attitude_setpoint_s &attitude_setpoint) const
{
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle,
omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_proj_axes, attitude_setpoint);
omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, attitude_setpoint);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}

14
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View File

@ -170,17 +170,19 @@ public:
* This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
* It needs to be executed by the attitude controller to achieve velocity and position tracking.
* @param att current attitude of the robot
* @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt
* @param omni_att_mode attitude mode for omnidirectional vehicles
* @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
* @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees)
* @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees)
* @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
* @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees)
* @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
* @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
* @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
* @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians)
* @param omni_att_rate the attitude change rate for mode=6
* @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
* @param attitude_setpoint reference to struct to fill up
*/
void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust,
float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch,
int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
const float omni_att_rate, const int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
private:
/**

6
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -187,7 +187,8 @@ private:
(ParamFloat<px4::params::OMNI_ATT_TLT_DIR>) _param_omni_att_tilt_dir,
(ParamFloat<px4::params::OMNI_ATT_ROLL>) _param_omni_att_roll,
(ParamFloat<px4::params::OMNI_ATT_PITCH>) _param_omni_att_pitch,
(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes
(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes,
(ParamFloat<px4::params::OMNI_ATT_RATE>) _param_omni_att_rate
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@ -686,7 +687,8 @@ MulticopterPositionControl::Run()
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
_tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_proj_axes.get(), attitude_setpoint);
_tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_att_rate.get(), _param_omni_proj_axes.get(),
attitude_setpoint);
// Part of landing logic: if ground-contact/maybe landed was detected, turn off
// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.

14
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -856,3 +856,17 @@ PARAM_DEFINE_FLOAT(OMNI_ATT_PITCH, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_INT32(OMNI_PROJ_AXES, 1);
/**
* Rate for change of attitude
*
* Specifies the rate in which the attitude can change in slow attitude mode
* (OMNI_ATT_MODE = 6), rotating the Z axis from the current attitude to the
* optimal attitude. The value is in degrees per position control loop.
*
* @min 0
* @decimal 3
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(OMNI_ATT_RATE, 0.5f);