diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 14:14:35 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-01 14:14:35 +0400 |
| commit | b7f2c89a1687de7ac38fb5b9bcf48afdc4e3317b (patch) | |
| tree | 449928965269e90eb7c46063f4dd2353b462e43b /src/modules/mavlink | |
| parent | 6827e45aee2f1a6e756c56a91b46152eb2ea5d08 (diff) | |
| parent | 5b302fef59354f536e83a0b14572d2f954a6e682 (diff) | |
| download | px4-firmware-b7f2c89a1687de7ac38fb5b9bcf48afdc4e3317b.tar.gz px4-firmware-b7f2c89a1687de7ac38fb5b9bcf48afdc4e3317b.tar.bz2 px4-firmware-b7f2c89a1687de7ac38fb5b9bcf48afdc4e3317b.zip | |
Merge branch 'master' into navigator_new_vector
Diffstat (limited to 'src/modules/mavlink')
| -rw-r--r-- | src/modules/mavlink/orb_listener.c | 37 |
1 files changed, 22 insertions, 15 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index e168dded3..6e177bc4d 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -350,20 +350,26 @@ l_input_rc(const struct listener *l) /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - if (gcs_link) - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, - 0, - (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, - (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, - (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, - (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, - (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, - (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, - (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, - (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, - 255); + if (gcs_link) { + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc_raw.timestamp / 1000, + i, + (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + rc_raw.rssi); + } + } } void @@ -492,7 +498,8 @@ l_actuator_outputs(const struct listener *l) if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number */, + l->arg /* port number - needs GCS support */, + /* QGC has port number support already */ act_outputs.output[0], act_outputs.output[1], act_outputs.output[2], |