Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-11 14:02:38 +02:00
parent 4c13c67504 0baa7927fa
commit 5400ea9815
1 changed files with 23 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+23 -32
View File
@@ -1051,10 +1051,16 @@ protected:
uint32_t mavlink_custom_mode; uint32_t mavlink_custom_mode;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
/* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR || if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_system.type == MAV_TYPE_OCTOROTOR) {
/* set number of valid outputs depending on vehicle type */ /* multirotors: set number of rotor outputs depending on type */
unsigned n; unsigned n;
switch (mavlink_system.type) { switch (mavlink_system.type) {
@@ -1071,59 +1077,44 @@ protected:
break; break;
} }
/* scale / assign outputs depending on system type */
float out[8];
for (unsigned i = 0; i < 8; i++) { for (unsigned i = 0; i < 8; i++) {
if (i < n) { if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { if (i < n) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ /* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else { } else {
/* send 0 when disarmed */ /* scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = 0.0f; out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} }
} else { } else {
out[i] = -1.0f; /* send 0 when disarmed */
out[i] = 0.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 { } else {
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
/* 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++) { for (unsigned i = 0; i < 8; i++) {
if (i != 3) { if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ /* scale 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); out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else { } else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
/* 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); out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} }
} }
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);
} }
} }
}; };