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