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 (unfiltered) roll and pitch angle estimation mode

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-07-26 01:23:53 -04:00
parent 0630abd01a
commit 3124e76aaa
2 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -87,6 +87,15 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
}
case 6: { // Estimate the optimal roll and pitch to counteract the wind (used for omnidirectional vehicles)
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
att_sp.thrust_body[2] = -thr_sp.length();
// Set the roll angle
omni_att_roll = att_sp.roll_body;
// Set the pitch angle
omni_att_pitch = att_sp.pitch_body;
break;
}

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

@ -143,8 +143,11 @@ private:
home_position_s _home_pos{}; /**< home position */
landing_gear_s _landing_gear{};
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
float _param_tilt_angle = 0, _param_tilt_dir = 0; // Keeping the last parameter values in degrees
float _tilt_angle = 0, _tilt_dir = 0; // Keeping the currently used values in radians
float _param_roll_angle = 0, _param_pitch_angle = 0; // Keeping the last parameter values in degrees
float _tilt_roll = 0, _tilt_pitch = 0; // Keeping the currently used values in radians
DEFINE_PARAMETERS(
// Position Control
@ -404,6 +407,14 @@ MulticopterPositionControl::parameters_update(bool force)
_tilt_angle = math::radians(_param_tilt_angle);
_tilt_dir = math::radians(_param_tilt_dir);
}
if (abs(_param_omni_att_roll.get() - _param_roll_angle) > FLT_EPSILON
|| abs(_param_omni_att_pitch.get() - _param_pitch_angle) > FLT_EPSILON) {
_param_roll_angle = _param_omni_att_roll.get();
_param_pitch_angle = _param_omni_att_pitch.get();
_tilt_roll = math::radians(_param_roll_angle);
_tilt_pitch = math::radians(_param_pitch_angle);
}
}
return OK;
@ -674,10 +685,8 @@ MulticopterPositionControl::Run()
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
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(),
_tilt_angle, _tilt_dir, omni_att_roll, omni_att_pitch, _param_omni_proj_axes.get(), attitude_setpoint);
_tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _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.