forked from Archive/PX4-Autopilot
mavlink: HIL fixes, send 0 when disarmed
This commit is contained in:
parent
cc0f237c16
commit
624ff01018
|
@ -778,65 +778,45 @@ protected:
|
|||
uint32_t mavlink_custom_mode;
|
||||
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
|
||||
/* HIL message as per MAVLink spec */
|
||||
/* set number of valid outputs depending on vehicle type */
|
||||
unsigned n;
|
||||
switch (mavlink_system.type) {
|
||||
case MAV_TYPE_QUADROTOR:
|
||||
n = 4;
|
||||
break;
|
||||
|
||||
case MAV_TYPE_HEXAROTOR:
|
||||
n = 6;
|
||||
break;
|
||||
|
||||
default:
|
||||
n = 8;
|
||||
break;
|
||||
}
|
||||
|
||||
/* scale / assign outputs depending on system type */
|
||||
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
-1,
|
||||
-1,
|
||||
-1,
|
||||
-1,
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
float out[8];
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
|
||||
-1,
|
||||
-1,
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
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;
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[6] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act->output[7] - 900.0f) / 600.0f) / 2.0f,
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
|
||||
} else {
|
||||
mavlink_msg_hil_controls_send(_channel,
|
||||
hrt_absolute_time(),
|
||||
(act->output[0] - 1500.0f) / 500.0f,
|
||||
(act->output[1] - 1500.0f) / 500.0f,
|
||||
(act->output[2] - 1500.0f) / 500.0f,
|
||||
(act->output[3] - 1000.0f) / 1000.0f,
|
||||
(act->output[4] - 1500.0f) / 500.0f,
|
||||
(act->output[5] - 1500.0f) / 500.0f,
|
||||
(act->output[6] - 1500.0f) / 500.0f,
|
||||
(act->output[7] - 1500.0f) / 500.0f,
|
||||
mavlink_base_mode,
|
||||
0);
|
||||
} else {
|
||||
/* send 0 when disarmed */
|
||||
out[i] = 0.0f;
|
||||
}
|
||||
} 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);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue