aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_SITL: cope with no _sitl state

Browse Source
This commit is contained in:
Andrew Tridgell 2023-12-23 16:07:40 +11:00
parent f5bc7f02a8
commit c1016ae52e
1 changed files with 16 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -327,12 +327,13 @@ void SITL_State::_output_to_flightgear(void)
*/ */
void SITL_State::_fdm_input_local(void) void SITL_State::_fdm_input_local(void)
{ {
if (_sitl == nullptr) {
return;
}
struct sitl_input input; struct sitl_input input;
// check for direct RC input // check for direct RC input
if (_sitl != nullptr) { _check_rc_input();
_check_rc_input();
}
// construct servos structure for FDM // construct servos structure for FDM
_simulator_servos(input); _simulator_servos(input);
@ -350,19 +351,17 @@ void SITL_State::_fdm_input_local(void)
sitl_model->update_model(input); sitl_model->update_model(input);
// get FDM output from the model // get FDM output from the model
if (_sitl) { sitl_model->fill_fdm(_sitl->state);
sitl_model->fill_fdm(_sitl->state);
#if HAL_NUM_CAN_IFACES #if HAL_NUM_CAN_IFACES
if (CANIface::num_interfaces() > 0) { if (CANIface::num_interfaces() > 0) {
multicast_state_send(); multicast_state_send();
} }
#endif #endif
if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) { if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) {
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;
}
} }
} }
@ -373,16 +372,12 @@ void SITL_State::_fdm_input_local(void)
sim_update(); sim_update();
if (_sitl && _use_fg_view) { if (_use_fg_view) {
_output_to_flightgear(); _output_to_flightgear();
} }
// update simulation time // update simulation time
if (_sitl) { hal.scheduler->stop_clock(_sitl->state.timestamp_us);
hal.scheduler->stop_clock(_sitl->state.timestamp_us);
} else {
hal.scheduler->stop_clock(AP_HAL::micros64()+100);
}
set_height_agl(); set_height_agl();
@ -395,6 +390,9 @@ void SITL_State::_fdm_input_local(void)
*/ */
void SITL_State::_simulator_servos(struct sitl_input &input) void SITL_State::_simulator_servos(struct sitl_input &input)
{ {
if (_sitl == nullptr) {
return;
}
static uint32_t last_update_usec; static uint32_t last_update_usec;
/* this maps the registers used for PWM outputs. The RC /* this maps the registers used for PWM outputs. The RC