mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
4d50889ec7
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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)
|
||||||
|
|
Loading…
Reference in New Issue