HAL_SITL: check for SIM_RC_FAIL
This commit is contained in:
parent
4e3dd315cb
commit
0166ab2038
@ -275,10 +275,12 @@ void SITL_State::_fdm_input_local(void)
|
|||||||
sitl_model->fill_fdm(_sitl->state);
|
sitl_model->fill_fdm(_sitl->state);
|
||||||
_sitl->update_rate_hz = sitl_model->get_rate_hz();
|
_sitl->update_rate_hz = sitl_model->get_rate_hz();
|
||||||
|
|
||||||
|
if (_sitl->rc_fail == 0) {
|
||||||
for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) {
|
for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) {
|
||||||
pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000;
|
pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (gimbal != NULL) {
|
if (gimbal != NULL) {
|
||||||
gimbal->update();
|
gimbal->update();
|
||||||
|
Loading…
Reference in New Issue
Block a user