fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 01:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into mpc_local_pos

Browse Source
This commit is contained in:
Anton Babushkin
2014-03-28 10:44:28 +04:00
parent 83da4ae02d 9b1de5004c
commit a991ebd8ca
11 changed files with 271 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+63 -30
View File
@@ -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);
}
}
}
};
src/modules/mavlink/mavlink_receiver.cpp
+3 -1
View File
@@ -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);
}
}