diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-28 10:44:28 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-28 10:44:28 +0400 |
commit | a991ebd8ca4aa1dc6fd69e307a74be6a93f4e6ff (patch) | |
tree | 6f96a4c1e8814cb9232b6b1f94a0f7be4d87116e /src/modules/mavlink | |
parent | 83da4ae02dfc61d6a7f80ae40660826fbbca81be (diff) | |
parent | 9b1de5004c673ebe8bdf68f1b518565cccd6b05b (diff) | |
download | px4-firmware-a991ebd8ca4aa1dc6fd69e307a74be6a93f4e6ff.tar.gz px4-firmware-a991ebd8ca4aa1dc6fd69e307a74be6a93f4e6ff.tar.bz2 px4-firmware-a991ebd8ca4aa1dc6fd69e307a74be6a93f4e6ff.zip |
Merge branch 'master' into mpc_local_pos
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 93 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 4 |
2 files changed, 66 insertions, 31 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 037999af7..4ca3840d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,6 +71,7 @@ #include <uORB/topics/battery_status.h> #include <uORB/topics/navigation_capabilities.h> #include <drivers/drv_rc_input.h> +#include <drivers/drv_pwm_output.h> #include "mavlink_messages.h" @@ -788,47 +789,79 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* set number of valid outputs depending on vehicle type */ - unsigned n; + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - default: - n = 8; - break; - } + default: + n = 8; + break; + } - /* scale / assign outputs depending on system type */ - float out[8]; + /* scale / assign outputs depending on system type */ + 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; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + 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); + } else { + + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake 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 fake 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 { - 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); + 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); + } } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2c9cdbf24..dc6236a51 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -635,13 +635,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } |