dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

extend uuv_att_control module by thrust in y/z-direction (#16642) 

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* update formatting

* fix submarine sitl: indicate motor channel range 0..1 or -1..1 in simulation_mavlink.cpp (#16637)

change motor_count variable to 'pos_thrust_motor_count'
This is more specific to what is actually happening in the code.
'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) all other motors are configured for -1..1 range

submarine only have motors with -1..1 range.
Thus, pos_thrust_motor_count = 0

Co-authored-by: Thies Lennart Alff <33184858+lennartalff@users.noreply.github.com>

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* update formatting

Co-authored-by: Thies Lennart Alff <33184858+lennartalff@users.noreply.github.com>
This commit is contained in:
Daniel Duecker 2021-01-27 22:35:24 +01:00 committed by GitHub
parent c033681c56
commit adf2d2f73a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 36 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -93,7 +93,8 @@ void UUVAttitudeControl::parameters_update(bool force)
}
}
void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u)
void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
float thrust_x, float thrust_y, float thrust_z)
{
if (PX4_ISFINITE(roll_u)) {
roll_u = math::constrain(roll_u, -1.0f, 1.0f);
@ -119,13 +120,29 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
_actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f;
}
if (PX4_ISFINITE(thrust_u)) {
thrust_u = math::constrain(thrust_u, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_u;
if (PX4_ISFINITE(thrust_x)) {
thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_x;
} else {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
}
if (PX4_ISFINITE(thrust_y)) {
thrust_y = math::constrain(thrust_y, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = thrust_y;
} else {
_actuators.control[actuator_controls_s::INDEX_FLAPS] = 0.0f;
}
if (PX4_ISFINITE(thrust_z)) {
thrust_z = math::constrain(thrust_z, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = thrust_z;
} else {
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = 0.0f;
}
}
void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,
@ -143,7 +160,9 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
float roll_u;
float pitch_u;
float yaw_u;
float thrust_u;
float thrust_x;
float thrust_y;
float thrust_z;
float roll_body = attitude_setpoint.roll_body;
float pitch_body = attitude_setpoint.pitch_body;
@ -191,9 +210,12 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
yaw_u = torques(2);
// take thrust as
thrust_u = attitude_setpoint.thrust_body[0];
thrust_x = attitude_setpoint.thrust_body[0];
thrust_y = attitude_setpoint.thrust_body[1];
thrust_z = attitude_setpoint.thrust_body[2];
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_u);
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
/* Geometric Controller END*/
}
@ -235,6 +257,8 @@ void UUVAttitudeControl::Run()
_attitude_setpoint.pitch_body = _param_direct_pitch.get();
_attitude_setpoint.yaw_body = _param_direct_yaw.get();
_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
_attitude_setpoint.thrust_body[1] = 0.f;
_attitude_setpoint.thrust_body[2] = 0.f;
}
/* Geometric Control*/
@ -242,7 +266,7 @@ void UUVAttitudeControl::Run()
if (skip_controller) {
constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw,
_rates_setpoint.thrust_body[0]);
_rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]);
} else {
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
@ -257,7 +281,8 @@ void UUVAttitudeControl::Run()
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */
constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
_manual_control_setpoint.r, _manual_control_setpoint.z);
_manual_control_setpoint.r,
_manual_control_setpoint.z, 0.f, 0.f);
}
}

3
src/modules/uuv_att_control/uuv_att_control.hpp
View File

@ -146,5 +146,6 @@ private:
*/
void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint,
const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint);
void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u);
void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
float thrust_x, float thrust_y, float thrust_z);
};