diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index d5e0fb9c7f..eb3f2156b7 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -27,6 +27,14 @@ #include #include +#ifdef SFML_JOYSTICK + #ifdef HAVE_SFML_GRAPHICS_HPP + #include + #elif HAVE_SFML_GRAPHIC_H + #include + #endif +#endif // SFML_JOYSTICK + extern const AP_HAL::HAL& hal; namespace SITL { @@ -253,6 +261,10 @@ const AP_Param::GroupInfo SITL::var_info3[] = { // user settable common airspeed parameters AP_GROUPINFO("ARSPD_SIGN", 62, SITL, arspd_signflip, 0), +#ifdef SFML_JOYSTICK + AP_SUBGROUPEXTENSION("", 63, SITL, var_sfml_joystick), +#endif // SFML_JOYSTICK + AP_GROUPEND }; @@ -332,6 +344,22 @@ const AP_Param::GroupInfo SITL::var_mag[] = { #endif AP_GROUPEND }; + +#ifdef SFML_JOYSTICK +const AP_Param::GroupInfo SITL::var_sfml_joystick[] = { + AP_GROUPINFO("SF_JS_STICK", 1, SITL, sfml_joystick_id, 0), + AP_GROUPINFO("SF_JS_AXIS1", 2, SITL, sfml_joystick_axis[0], sf::Joystick::Axis::X), + AP_GROUPINFO("SF_JS_AXIS2", 3, SITL, sfml_joystick_axis[1], sf::Joystick::Axis::Y), + AP_GROUPINFO("SF_JS_AXIS3", 4, SITL, sfml_joystick_axis[2], sf::Joystick::Axis::Z), + AP_GROUPINFO("SF_JS_AXIS4", 5, SITL, sfml_joystick_axis[3], sf::Joystick::Axis::U), + AP_GROUPINFO("SF_JS_AXIS5", 6, SITL, sfml_joystick_axis[4], sf::Joystick::Axis::V), + AP_GROUPINFO("SF_JS_AXIS6", 7, SITL, sfml_joystick_axis[5], sf::Joystick::Axis::R), + AP_GROUPINFO("SF_JS_AXIS7", 8, SITL, sfml_joystick_axis[6], sf::Joystick::Axis::PovX), + AP_GROUPINFO("SF_JS_AXIS8", 9, SITL, sfml_joystick_axis[7], sf::Joystick::Axis::PovY), + AP_GROUPEND +}; + +#endif //SFML_JOYSTICK /* report SITL state via MAVLink */ void SITL::simstate_send(mavlink_channel_t chan) diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3992bad51d..21467537c6 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -95,6 +95,9 @@ public: AP_Param::setup_object_defaults(this, var_info3); AP_Param::setup_object_defaults(this, var_gps); AP_Param::setup_object_defaults(this, var_mag); +#ifdef SFML_JOYSTICK + AP_Param::setup_object_defaults(this, var_sfml_joystick); +#endif // SFML_JOYSTICK if (_singleton != nullptr) { AP_HAL::panic("Too many SITL instances"); } @@ -143,6 +146,9 @@ public: static const struct AP_Param::GroupInfo var_info3[]; static const struct AP_Param::GroupInfo var_gps[]; static const struct AP_Param::GroupInfo var_mag[]; +#ifdef SFML_JOYSTICK + static const struct AP_Param::GroupInfo var_sfml_joystick[]; +#endif //SFML_JOYSTICK // Board Orientation (and inverse) Matrix3f ahrs_rotation; @@ -224,6 +230,11 @@ public: AP_Float buoyancy; // submarine buoyancy in Newtons AP_Int16 loop_rate_hz; +#ifdef SFML_JOYSTICK + AP_Int8 sfml_joystick_id; + AP_Int8 sfml_joystick_axis[8]; +#endif + // EFI type enum EFIType { EFI_TYPE_NONE = 0,