This commit is contained in:
Amilcar Lucas 2011-10-09 01:35:06 +02:00
commit 4d50889ec7
3 changed files with 26 additions and 7 deletions

View File

@ -773,7 +773,7 @@ static void fifty_hz_loop()
sonar_alt = sonar.read(); 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 for a copter needs very fast update of the servo values
hil.send_message(MSG_RADIO_OUT); hil.send_message(MSG_RADIO_OUT);
#endif #endif

View File

@ -989,6 +989,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} }
#endif #endif
*/
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
case MAVLINK_MSG_ID_RAW_IMU: case MAVLINK_MSG_ID_RAW_IMU:
@ -1036,7 +1037,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} }
#endif // HIL_MODE #endif // HIL_MODE
*/
} // end switch } // end switch
} // end handle mavlink } // end handle mavlink

View File

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