diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-02 12:53:46 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-02 12:53:46 +0400 |
commit | 624ff010185c16dffacc8d000aae2748ff1cd0a1 (patch) | |
tree | 38d3ab13d8faf6377c6ef1883022cd3f7f8c38fb /src/modules/mavlink | |
parent | cc0f237c1685c1d977fac2e69c8a1afb000f0919 (diff) | |
download | px4-firmware-624ff010185c16dffacc8d000aae2748ff1cd0a1.tar.gz px4-firmware-624ff010185c16dffacc8d000aae2748ff1cd0a1.tar.bz2 px4-firmware-624ff010185c16dffacc8d000aae2748ff1cd0a1.zip |
mavlink: HIL fixes, send 0 when disarmed
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 92 |
1 files changed, 36 insertions, 56 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8be7d76d7..c22289772 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,65 +778,45 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* HIL message as per MAVLink spec */ + /* set number of valid outputs depending on vehicle type */ + unsigned n; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } /* scale / assign outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } else { + out[i] = -1.0f; + } } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } }; |