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

mc_att_ctrl: added yawrate control from offboard. 

This commit adds Roll Pitch Yawrate Thrust (RPYrT) setpoint control to the
PX4 stack, enabling the UAV to follow specific yawrates sent from
offboard. It also introduces individual body_rate flags, along the
lines of mavros.

Tested on a MoCap enabled flight arena in KTH Royal Institute of
Technology, Stockholm. The MAV receives RPYrT setpoints from an
external PID controller to stabilize around position setpoints.
The UAV is also externally disturbed to assess the stability to
external unmodeled events.

Fixed Kabir comments.

Removed deprecated ignore_bodyrate.

Fixed integration test.
This commit is contained in:
pedro-roque 2019-05-23 15:24:12 +02:00 committed by Julian Oes
parent ac002db25c
commit a707403eaf
4 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
integrationtests/python_src/px4_it/util/manual_input.py
View File

@ -129,7 +129,9 @@ class ManualInput(object):
mode = offboard_control_mode()
mode.ignore_thrust = ignore_thrust
mode.ignore_attitude = ignore_attitude
mode.ignore_bodyrate = ignore_bodyrate
mode.ignore_bodyrate_x = ignore_bodyrate
mode.ignore_bodyrate_y = ignore_bodyrate
mode.ignore_bodyrate_z = ignore_bodyrate
mode.ignore_position = ignore_position
mode.ignore_velocity = ignore_velocity
mode.ignore_acceleration_force = ignore_acceleration_force

1
msg/offboard_control_mode.msg
View File

@ -4,7 +4,6 @@ uint64 timestamp # time since system start (microseconds)
bool ignore_thrust
bool ignore_attitude
bool ignore_bodyrate
bool ignore_bodyrate_x
bool ignore_bodyrate_y
bool ignore_bodyrate_z

27
src/modules/commander/Commander.cpp
View File

@ -1516,7 +1516,9 @@ Commander::run()
if (old.ignore_thrust != offboard_control_mode.ignore_thrust ||
old.ignore_attitude != offboard_control_mode.ignore_attitude ||
old.ignore_bodyrate != offboard_control_mode.ignore_bodyrate ||
old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x ||
old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y ||
old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z ||
old.ignore_position != offboard_control_mode.ignore_position ||
old.ignore_velocity != offboard_control_mode.ignore_velocity ||
old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force ||
@ -3323,24 +3325,25 @@ set_control_mode()
* Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
*/
control_mode.flag_control_rates_enabled =
!offboard_control_mode.ignore_bodyrate_x ||
!offboard_control_mode.ignore_bodyrate_y ||
!offboard_control_mode.ignore_bodyrate_z ||
!offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
!offboard_control_mode.ignore_bodyrate_x ||
!offboard_control_mode.ignore_bodyrate_y ||
!offboard_control_mode.ignore_bodyrate_z ||
!offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
// TO-DO: Add support for other modes than yawrate control
control_mode.flag_control_yawrate_override_enabled =
offboard_control_mode.ignore_bodyrate_x &&
offboard_control_mode.ignore_bodyrate_y &&
!offboard_control_mode.ignore_bodyrate_z &&
!offboard_control_mode.ignore_attitude;
offboard_control_mode.ignore_bodyrate_x &&
offboard_control_mode.ignore_bodyrate_y &&
!offboard_control_mode.ignore_bodyrate_z &&
!offboard_control_mode.ignore_attitude;
control_mode.flag_control_rattitude_enabled = false;

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

@ -825,8 +825,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
/* yawrate ignore flag mapps to ignore_bodyrate */
// offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800);
@ -1374,7 +1372,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
///XXX add support for ignoring individual axes
if (!_offboard_control_mode.ignore_bodyrate_x ||
!_offboard_control_mode.ignore_bodyrate_y ||
!_offboard_control_mode.ignore_bodyrate_z) {