mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 00:13:59 -04:00
SITL: correct adsb initialisation
Parameters have not yet been initialised in _sitl_setup, so move should-run checks into update
This commit is contained in:
parent
569443231a
commit
309ac53806
@ -63,6 +63,8 @@ void SITL_State::_set_param_default(const char *parm)
|
||||
*/
|
||||
void SITL_State::_sitl_setup(const char *home_str)
|
||||
{
|
||||
_home_str = home_str;
|
||||
|
||||
#ifndef __CYGWIN__
|
||||
_parent_pid = getppid();
|
||||
#endif
|
||||
@ -97,10 +99,6 @@ void SITL_State::_sitl_setup(const char *home_str)
|
||||
gimbal = new SITL::Gimbal(_sitl->state);
|
||||
}
|
||||
|
||||
if (_sitl->adsb_plane_count >= 0) {
|
||||
adsb = new SITL::ADSB(_sitl->state, home_str);
|
||||
}
|
||||
|
||||
fg_socket.connect("127.0.0.1", 5503);
|
||||
}
|
||||
|
||||
@ -173,6 +171,15 @@ void SITL_State::_fdm_input_step(void)
|
||||
_update_barometer(_sitl->state.altitude);
|
||||
_update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
|
||||
_update_flow();
|
||||
|
||||
if (_sitl->adsb_plane_count >= 0 &&
|
||||
adsb == nullptr) {
|
||||
adsb = new SITL::ADSB(_sitl->state, _home_str);
|
||||
} else if (_sitl->adsb_plane_count == -1 &&
|
||||
adsb != nullptr) {
|
||||
delete adsb;
|
||||
adsb = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
// trigger all APM timers.
|
||||
|
@ -222,6 +222,8 @@ private:
|
||||
const char *_client_address;
|
||||
|
||||
const char *defaults_path = HAL_PARAM_DEFAULTS_PATH;
|
||||
|
||||
const char *_home_str;
|
||||
};
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
Loading…
Reference in New Issue
Block a user