zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add nan protection to attitude and actuator targets

sbg
Andreas Antener 9 years ago
parent
b1ee89b5ca
commit
46ad873d00
1 changed files with 41 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 61
      src/modules/mavlink/mavlink_receiver.cpp

61
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -642,16 +642,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t @@ -642,16 +642,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
struct offboard_control_mode_s offboard_control_mode;
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) &&
PX4_ISFINITE(set_position_target_local_ned.y) &&
PX4_ISFINITE(set_position_target_local_ned.z) &&
PX4_ISFINITE(set_position_target_local_ned.vx) &&
PX4_ISFINITE(set_position_target_local_ned.vy) &&
PX4_ISFINITE(set_position_target_local_ned.vz) &&
PX4_ISFINITE(set_position_target_local_ned.afx) &&
PX4_ISFINITE(set_position_target_local_ned.afy) &&
PX4_ISFINITE(set_position_target_local_ned.afz) &&
PX4_ISFINITE(set_position_target_local_ned.yaw);
bool values_finite =
PX4_ISFINITE(set_position_target_local_ned.x) &&
PX4_ISFINITE(set_position_target_local_ned.y) &&
PX4_ISFINITE(set_position_target_local_ned.z) &&
PX4_ISFINITE(set_position_target_local_ned.vx) &&
PX4_ISFINITE(set_position_target_local_ned.vy) &&
PX4_ISFINITE(set_position_target_local_ned.vz) &&
PX4_ISFINITE(set_position_target_local_ned.afx) &&
PX4_ISFINITE(set_position_target_local_ned.afy) &&
PX4_ISFINITE(set_position_target_local_ned.afz) &&
PX4_ISFINITE(set_position_target_local_ned.yaw);
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
@ -763,14 +764,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t @@ -763,14 +764,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
}
//XXX handle global pos setpoints (different MAV frames)
if (valid) {
if (_pos_sp_triplet_pub == nullptr) {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
&pos_sp_triplet);
} else {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
&pos_sp_triplet);
}
if (_pos_sp_triplet_pub == nullptr) {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
&pos_sp_triplet);
} else {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
&pos_sp_triplet);
}
}
@ -793,10 +792,21 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m @@ -793,10 +792,21 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
bool values_finite =
PX4_ISFINITE(set_actuator_control_target.controls[0]) &&
PX4_ISFINITE(set_actuator_control_target.controls[1]) &&
PX4_ISFINITE(set_actuator_control_target.controls[2]) &&
PX4_ISFINITE(set_actuator_control_target.controls[3]) &&
PX4_ISFINITE(set_actuator_control_target.controls[4]) &&
PX4_ISFINITE(set_actuator_control_target.controls[5]) &&
PX4_ISFINITE(set_actuator_control_target.controls[6]) &&
PX4_ISFINITE(set_actuator_control_target.controls[7]);
if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
set_actuator_control_target.target_system == 0) &&
(mavlink_system.compid == set_actuator_control_target.target_component ||
set_actuator_control_target.target_component == 0)) {
set_actuator_control_target.target_component == 0) &&
values_finite) {
/* ignore all since we are setting raw actuators here */
offboard_control_mode.ignore_thrust = true;
@ -886,11 +896,22 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) @@ -886,11 +896,22 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
bool values_finite =
PX4_ISFINITE(set_attitude_target.q[0]) &&
PX4_ISFINITE(set_attitude_target.q[1]) &&
PX4_ISFINITE(set_attitude_target.q[2]) &&
PX4_ISFINITE(set_attitude_target.q[3]) &&
PX4_ISFINITE(set_attitude_target.thrust) &&
PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
PX4_ISFINITE(set_attitude_target.body_yaw_rate);
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_attitude_target.target_system ||
set_attitude_target.target_system == 0) &&
(mavlink_system.compid == set_attitude_target.target_component ||
set_attitude_target.target_component == 0)) {
set_attitude_target.target_component == 0) &&
values_finite) {
/* set correct ignore flags for thrust field: copy from mavlink message */
_offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));

Write Preview
Loading…
Cancel
Save