AP_WindVane: Enable SITL when it is selected

This commit is contained in:
murata 2022-01-30 14:21:17 +09:00 committed by Peter Barker
parent dcafaf3ab2
commit 5825ab4dc6
2 changed files with 8 additions and 4 deletions

View File

@ -209,12 +209,12 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
case WindVaneType::WINDVANE_ANALOG_PIN: case WindVaneType::WINDVANE_ANALOG_PIN:
_direction_driver = new AP_WindVane_Analog(*this); _direction_driver = new AP_WindVane_Analog(*this);
break; break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case WindVaneType::WINDVANE_SITL_TRUE: case WindVaneType::WINDVANE_SITL_TRUE:
case WindVaneType::WINDVANE_SITL_APPARENT: case WindVaneType::WINDVANE_SITL_APPARENT:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
_direction_driver = new AP_WindVane_SITL(*this); _direction_driver = new AP_WindVane_SITL(*this);
#endif
break; break;
#endif
case WindVaneType::WINDVANE_NMEA: case WindVaneType::WINDVANE_NMEA:
_direction_driver = new AP_WindVane_NMEA(*this); _direction_driver = new AP_WindVane_NMEA(*this);
_direction_driver->init(serial_manager); _direction_driver->init(serial_manager);
@ -231,17 +231,17 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
case Speed_type::WINDVANE_WIND_SENSOR_REV_P: case Speed_type::WINDVANE_WIND_SENSOR_REV_P:
_speed_driver = new AP_WindVane_ModernDevice(*this); _speed_driver = new AP_WindVane_ModernDevice(*this);
break; break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Speed_type::WINDSPEED_SITL_TRUE: case Speed_type::WINDSPEED_SITL_TRUE:
case Speed_type::WINDSPEED_SITL_APPARENT: case Speed_type::WINDSPEED_SITL_APPARENT:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// single driver does both speed and direction // single driver does both speed and direction
if (_direction_type != _speed_sensor_type) { if (_direction_type != _speed_sensor_type) {
_speed_driver = new AP_WindVane_SITL(*this); _speed_driver = new AP_WindVane_SITL(*this);
} else { } else {
_speed_driver = _direction_driver; _speed_driver = _direction_driver;
} }
#endif
break; break;
#endif
case Speed_type::WINDSPEED_NMEA: case Speed_type::WINDSPEED_NMEA:
// single driver does both speed and direction // single driver does both speed and direction
if (_direction_type != WindVaneType::WINDVANE_NMEA) { if (_direction_type != WindVaneType::WINDVANE_NMEA) {

View File

@ -166,8 +166,10 @@ private:
WINDVANE_PWM_PIN = 2, WINDVANE_PWM_PIN = 2,
WINDVANE_ANALOG_PIN = 3, WINDVANE_ANALOG_PIN = 3,
WINDVANE_NMEA = 4, WINDVANE_NMEA = 4,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
WINDVANE_SITL_TRUE = 10, WINDVANE_SITL_TRUE = 10,
WINDVANE_SITL_APPARENT = 11, WINDVANE_SITL_APPARENT = 11,
#endif
}; };
enum Speed_type { enum Speed_type {
@ -176,8 +178,10 @@ private:
WINDVANE_WIND_SENSOR_REV_P = 2, WINDVANE_WIND_SENSOR_REV_P = 2,
WINDSPEED_RPM = 3, WINDSPEED_RPM = 3,
WINDSPEED_NMEA = 4, WINDSPEED_NMEA = 4,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
WINDSPEED_SITL_TRUE = 10, WINDSPEED_SITL_TRUE = 10,
WINDSPEED_SITL_APPARENT = 11, WINDSPEED_SITL_APPARENT = 11,
#endif
}; };
static AP_WindVane *_singleton; static AP_WindVane *_singleton;