diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f574099119..6f4d76e752 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index fea60d35c4..9bc57ca801 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -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)