diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 18a3b9e2baf6b..d00a99b43d675 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -70,6 +70,10 @@ void AP_Mount_Viewpro::update() send_comm_config_cmd(CommConfigCmd::QUERY_FIRMWARE_VER); } + if (!_init_rangefinder_enable) { + init_rangefinder_enable(); + } + // send handshake send_handshake(); @@ -946,4 +950,19 @@ bool AP_Mount_Viewpro::set_rangefinder_enable(bool enable) return send_camera_command(ImageSensor::NO_ACTION, CameraCommand::NO_ACTION, 0, enable ? LRFCommand::CONTINUOUS_RANGING_START : LRFCommand::STOP_RANGING); } +// initialise rangefinder enable/disable state +void AP_Mount_Viewpro::init_rangefinder_enable() +{ +#if AP_SCRIPTING_ENABLED + // get cached aux switch position and initialise rangefinder enable state + uint8_t sw_pos; + if (rc().get_aux_cached(RC_Channel::AUX_FUNC::MOUNT_LRF_ENABLE, sw_pos)) { + _init_rangefinder_enable = set_rangefinder_enable(sw_pos == (uint8_t)RC_Channel::AuxSwitchPos::HIGH); + return; + } +#endif + + _init_rangefinder_enable = true; +} + #endif // HAL_MOUNT_VIEWPRO_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index da74517f1b047..84fd6b8e30100 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -373,6 +373,9 @@ class AP_Mount_Viewpro : public AP_Mount_Backend // send vehicle attitude and position to gimbal bool send_m_ahrs(); + // initialise rangefinder enable/disable state + void init_rangefinder_enable(); + // internal variables AP_HAL::UARTDriver *_uart; // uart connected to gimbal bool _initialised; // true once the driver has been initialised @@ -403,6 +406,7 @@ class AP_Mount_Viewpro : public AP_Mount_Backend bool _got_firmware_version; // true once we have received the firmware version uint8_t _model_name[11] {}; // model name received from gimbal bool _got_model_name; // true once we have received model name + bool _init_rangefinder_enable; // true once rangefinder enable/disabled state has been initialised float _rangefinder_dist_m; // latest rangefinder distance (in meters) };