Merge pull request #1272 from PX4/mavlink_fixes

Mavlink fixes

2 files changed, 15 insertions, 10 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9a39d3bed..408605939 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
 		configure_stream("RC_CHANNELS_RAW", 1.0f);
-		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
-		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
+		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c10be77b5..f292605c9 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1333,7 +1333,7 @@ protected:
 				}
 
 				for (unsigned i = 0; i < 8; i++) {
-					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+					if (act.output[i] > PWM_LOWEST_MIN / 2) {
 						if (i < n) {
 							/* scale PWM out 900..2100 us to 0..1 for rotors */
 							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
@@ -1344,7 +1344,7 @@ protected:
 						}
 
 					} else {
-						/* send 0 when disarmed */
+						/* send 0 when disarmed and for disabled channels */
 						out[i] = 0.0f;
 					}
 				}
@@ -1353,15 +1353,20 @@ protected:
 				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
 
 				for (unsigned i = 0; i < 8; i++) {
-					if (i != 3) {
-						/* scale PWM out 900..2100 us to -1..1 for normal channels */
-						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+					if (act.output[i] > PWM_LOWEST_MIN / 2) {
+						if (i != 3) {
+							/* scale PWM out 900..2100 us to -1..1 for normal channels */
+							out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+						} else {
+							/* scale PWM out 900..2100 us to 0..1 for throttle */
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						}
 
 					} else {
-						/* scale PWM out 900..2100 us to 0..1 for throttle */
-						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						/* set 0 for disabled channels */
+						out[i] = 0.0f;
 					}
-
 				}
 			}