diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-11 16:48:33 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-11 16:48:33 +0100 |
commit | 8edf02681bc4cce3a5ee315841d14594166b6b31 (patch) | |
tree | b6d4a110e1774df8ef15a57e2f5c806e7b8249b1 | |
parent | 2b1e199b91641fb3ad097792f5ead98d5148de00 (diff) | |
parent | 7d8d7a76b986e7acefb4a61f3da3625db1f6dd11 (diff) | |
download | px4-firmware-8edf02681bc4cce3a5ee315841d14594166b6b31.tar.gz px4-firmware-8edf02681bc4cce3a5ee315841d14594166b6b31.tar.bz2 px4-firmware-8edf02681bc4cce3a5ee315841d14594166b6b31.zip |
merge origin master into fw_control
-rw-r--r-- | apps/mavlink/mavlink.c | 15 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 96 |
2 files changed, 78 insertions, 33 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 460faf446..2527e0b0f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -132,6 +132,7 @@ static struct mavlink_logbuffer lb; static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); @@ -161,15 +162,13 @@ set_hil_on_off(bool hil_enabled) } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; - } else if (baudrate < 460800) { - /* 50 Hz */ - hil_rate_interval = 20; } else { /* 100 Hz */ hil_rate_interval = 10; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { @@ -263,7 +262,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } -static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -453,19 +452,19 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +mavlink_status_t* mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; + return &m_mavlink_status[channel]; } /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[chan]; + return &m_mavlink_buffer[channel]; } void mavlink_update_system(void) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index c49b47051..3ac229d73 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -441,33 +441,79 @@ l_actuator_outputs(struct listener *l) uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - float rudder, throttle; - - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 1500.0f) / 600.0f) + 1.0f) / 2.0f; + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 1500.0f) / 600.0f) + 1.0f) / 2.0f; + float rudder, throttle; + + /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + + // XXX very ugly, needs rework + if (isfinite(act_outputs.output[3]) + && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { + /* throttle is fourth output */ + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + } else { + /* only three outputs, put throttle on position 4 / index 3 */ + rudder = 0; + throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + } + + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 600.0f, + (act_outputs.output[1] - 1500.0f) / 600.0f, + rudder, + throttle, + (act_outputs.output[4] - 1500.0f) / 600.0f, + (act_outputs.output[5] - 1500.0f) / 600.0f, + (act_outputs.output[6] - 1500.0f) / 600.0f, + (act_outputs.output[7] - 1500.0f) / 600.0f, + mavlink_mode, + 0); } - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, - mavlink_mode, - 0); } } } |