This commit is contained in:
Michael Oborne 2011-09-27 07:30:28 +08:00
parent 30f4ced9df
commit 2de9e6a929
2 changed files with 20 additions and 1 deletions

View File

@ -769,7 +769,7 @@ static void fifty_hz_loop()
sonar_alt = sonar.read();
}
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
// HIL for a copter needs very fast update of the servo values
hil.send_message(MSG_RADIO_OUT);
#endif

View File

@ -135,6 +135,23 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
#if FRAME_CONFIG == HELI_FRAME
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out
0,
0,
0,
0,
rssi);
#else
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.rc_1.norm_output(),
@ -146,6 +163,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
0,
0,
rssi);
#endif
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)