diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 065e7b8822..37e4b40258 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -14,6 +14,7 @@ void AP_Mount_Alexmos::init() read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get()); } + AP_Mount_Backend::init(); } // update mount position - should be called periodically diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index c161315c77..fc367261f0 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -7,6 +7,13 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate +// Default init function for every mount +void AP_Mount_Backend::init() +{ + // setting default target sysid from parameters + _target_sysid = _params.sysid_default.get(); +} + // set angle target in degrees // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 8b954e90c7..b44d6e71b0 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -35,7 +35,7 @@ public: {} // init - performs any required initialisation for this instance - virtual void init() = 0; + virtual void init(); // update mount position - should be called periodically virtual void update() = 0; diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index d97204e7bd..84670366e5 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -18,9 +18,6 @@ public: // Constructor using AP_Mount_Backend::AP_Mount_Backend; - // init - void init() override {} - // update mount position void update() override; diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index 1abfd4c705..b2d94f2278 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -152,6 +152,12 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @User: Standard AP_GROUPINFO("_LEAD_PTCH", 13, AP_Mount_Params, pitch_stb_lead, 0.0f), + // @Param: _SYSID_DFLT + // @DisplayName: Mount Target sysID + // @Description: Default Target sysID for the mount to point to + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO("_SYSID_DFLT", 14, AP_Mount_Params, sysid_default, 0), AP_GROUPEND }; diff --git a/libraries/AP_Mount/AP_Mount_Params.h b/libraries/AP_Mount/AP_Mount_Params.h index ce2a789b87..ec5e2851a2 100644 --- a/libraries/AP_Mount/AP_Mount_Params.h +++ b/libraries/AP_Mount/AP_Mount_Params.h @@ -29,4 +29,5 @@ public: AP_Float roll_stb_lead; // roll lead control gain (only used by servo backend) AP_Float pitch_stb_lead; // pitch lead control gain (only used by servo backend) + AP_Int8 sysid_default; // target sysid for mount to follow }; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index ef5fd7ded9..d850869f9f 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -17,9 +17,6 @@ public: // Constructor using AP_Mount_Backend::AP_Mount_Backend; - // init - performs any required initialisation for this instance - void init() override {} - // update mount position - should be called periodically void update() override; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index fec935ef32..599019aff7 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -16,6 +16,7 @@ void AP_Mount_SToRM32_serial::init() _initialised = true; set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get()); } + AP_Mount_Backend::init(); } diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index 323b0b19ee..d5f945fa12 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -22,9 +22,6 @@ public: /* Do not allow copies */ CLASS_NO_COPY(AP_Mount_Scripting); - // init - performs any required initialisation for this instance - void init() override {}; - // update mount position - should be called periodically void update() override; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index da43272e60..e4fea5d7b4 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -21,6 +21,7 @@ void AP_Mount_Servo::init() _pan_idx = SRV_Channel::k_mount2_pan; _open_idx = SRV_Channel::k_mount2_open; } + AP_Mount_Backend::init(); } // update mount position - should be called periodically diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 781d190fd6..3166e0e8bb 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -33,7 +33,7 @@ void AP_Mount_Siyi::init() _initialised = true; set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get()); } - + AP_Mount_Backend::init(); } // update mount position - should be called periodically diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 7334f513ae..7865ae1135 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -16,6 +16,7 @@ void AP_Mount_SoloGimbal::init() { _initialised = true; set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get()); + AP_Mount_Backend::init(); } void AP_Mount_SoloGimbal::update_fast()