fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 19:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Pos-Ctrl: Added the OMNI_PROJ_AXES parameter

Browse Source
This commit is contained in:
Azarakhsh Keipour
2020-07-13 19:35:08 -04:00
parent 68005486a1
commit 7a07f41b65
4 changed files with 20 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_params.c
+14
View File
@@ -241,3 +241,17 @@ PARAM_DEFINE_FLOAT(OMNI_ATT_ROLL, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(OMNI_ATT_PITCH, 0.0f);
/**
* Axes for thrust projection in omni-directional attitude modes
*
* Specifies the axis used for projecting inertial thrust setpoint
* to the body-fixed frame in omni-directional modes (OMNI_ATT_MODE > 0).
*
* @min 0
* @max 1
* @value 0 commanded attitude
* @value 1 current attitude
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_INT32(OMNI_PROJ_AXES, 1);
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+2 -2
View File
@@ -363,9 +363,9 @@ 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, vehicle_attitude_setpoint_s &attitude_setpoint) const
float &omni_att_pitch, 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, 1, attitude_setpoint);
omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_proj_axes, attitude_setpoint);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+1 -1
View File
@@ -180,7 +180,7 @@ public:
*/
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,
vehicle_attitude_setpoint_s &attitude_setpoint) const;
int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
private:
/**
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -2
View File
@@ -180,7 +180,8 @@ private:
(ParamFloat<px4::params::OMNI_ATT_TLT_ANG>) _param_omni_att_tilt_angle,
(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
(ParamFloat<px4::params::OMNI_ATT_PITCH>) _param_omni_att_pitch,
(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -667,7 +668,7 @@ MulticopterPositionControl::Run()
float omni_att_roll = _param_omni_att_roll.get();
float omni_att_pitch = _param_omni_att_pitch.get();
_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
omni_att_tilt_angle, omni_att_tilt_dir, omni_att_roll, omni_att_pitch, attitude_setpoint);
omni_att_tilt_angle, omni_att_tilt_dir, omni_att_roll, omni_att_pitch, _param_omni_proj_axes.get(), attitude_setpoint);
// Update the tilt angle and direciton parameters if we are in tilt estimation mode
if (_param_omni_att_mode.get() == 5) {