5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

Plane: fixed disarmed throttle in HIL

This commit is contained in:
Andrew Tridgell 2014-01-20 19:40:11 +11:00
parent 776b999f43
commit c16e3a035f

View File

@ -855,16 +855,6 @@ static void set_servos(void)
channel_rudder->radio_out = channel_rudder->radio_in;
}
#if HIL_MODE != HIL_MODE_DISABLED
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN) {
send_radio_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
#endif
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);
} else if (g.elevon_output != MIXING_DISABLED) {
@ -889,6 +879,16 @@ static void set_servos(void)
}
}
#if HIL_MODE != HIL_MODE_DISABLED
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
#endif
// send values to the PWM timers for output
// ----------------------------------------
channel_roll->output();