fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 17:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

merge origin master into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-11 16:48:33 +01:00
parent 2b1e199b91 7d8d7a76b9
commit 8edf02681b
2 changed files with 79 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+7 -8
View File
@@ -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)
apps/mavlink/orb_listener.c
+72 -26
View File
@@ -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;
} 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;
}
/* 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);
/* 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 {
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);
}
}
}
}