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

MAVLink/Commander: @Pedro-Roque's offboard yawrate handling

This commit is contained in:
Matthias Grob 2019-05-24 08:01:56 +01:00 committed by Julian Oes
parent 579cbbb42c
commit ac002db25c
5 changed files with 59 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/offboard_control_mode.msg
View File

@ -5,6 +5,9 @@ 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
bool ignore_position
bool ignore_velocity
bool ignore_acceleration_force

1
msg/vehicle_control_mode.msg
View File

@ -8,6 +8,7 @@ bool flag_control_auto_enabled # true if onboard autopilot should act
bool flag_control_offboard_enabled # true if offboard control should be used
bool flag_control_rates_enabled # true if rates are stabilized
bool flag_control_attitude_enabled # true if attitude stabilization is mixed in
bool flag_control_yawrate_override_enabled # true if the yaw rate override is enabled
bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled
bool flag_control_force_enabled # true if force control is mixed in
bool flag_control_acceleration_enabled # true if acceleration is controlled

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

@ -3322,7 +3322,10 @@ set_control_mode()
* The control flags depend on what is ignored according to the offboard control mode topic
* 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 ||
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 ||
@ -3333,6 +3336,12 @@ set_control_mode()
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
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;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&

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

@ -826,8 +826,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* 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 = (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);
bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
@ -938,7 +940,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
}
/* set the yawrate sp value */
if (!offboard_control_mode.ignore_bodyrate) {
if (!(offboard_control_mode.ignore_bodyrate_x ||
offboard_control_mode.ignore_bodyrate_y ||
offboard_control_mode.ignore_bodyrate_z)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
@ -994,7 +998,9 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
/* ignore all since we are setting raw actuators here */
offboard_control_mode.ignore_thrust = true;
offboard_control_mode.ignore_attitude = true;
offboard_control_mode.ignore_bodyrate = true;
offboard_control_mode.ignore_bodyrate_x = true;
offboard_control_mode.ignore_bodyrate_y = true;
offboard_control_mode.ignore_bodyrate_z = true;
offboard_control_mode.ignore_position = true;
offboard_control_mode.ignore_velocity = true;
offboard_control_mode.ignore_acceleration_force = true;
@ -1289,16 +1295,28 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
* throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
* body rates to keep the controllers running
*/
bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7);
bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & 0x1);
bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & 0x2);
bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & 0x4);
bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y ||
ignore_bodyrate_msg_z) &&
ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
_offboard_control_mode.ignore_bodyrate_x =
ignore_bodyrate_msg_x && _offboard_control_mode.ignore_bodyrate_x;
_offboard_control_mode.ignore_bodyrate_y =
ignore_bodyrate_msg_y && _offboard_control_mode.ignore_bodyrate_y;
_offboard_control_mode.ignore_bodyrate_z =
ignore_bodyrate_msg_z && _offboard_control_mode.ignore_bodyrate_z;
_offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
} else {
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
_offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x;
_offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y;
_offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z;
_offboard_control_mode.ignore_attitude = ignore_attitude_msg;
}
@ -1357,13 +1375,22 @@ 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)) {
if (!_offboard_control_mode.ignore_bodyrate_x ||
!_offboard_control_mode.ignore_bodyrate_y ||
!_offboard_control_mode.ignore_bodyrate_z) {
vehicle_rates_setpoint_s rates_sp = {};
rates_sp.timestamp = hrt_absolute_time();
if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
// only copy att rates sp if message contained new data
if (!ignore_bodyrate_msg_x) {
rates_sp.roll = set_attitude_target.body_roll_rate;
}
if (!ignore_bodyrate_msg_y) {
rates_sp.pitch = set_attitude_target.body_pitch_rate;
}
if (!ignore_bodyrate_msg_z) {
rates_sp.yaw = set_attitude_target.body_yaw_rate;
}

8
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -837,6 +837,14 @@ MulticopterAttitudeControl::run()
}
control_attitude();
if (_v_control_mode.flag_control_yawrate_override_enabled)
{
/* Yaw rate override enabled, overwrite the yaw setpoint */
vehicle_rates_setpoint_poll();
const auto yawrate_reference = _v_rates_sp.yaw;
_rates_sp(2) = yawrate_reference;
}
publish_rates_setpoint();
}