AP_Mount: ViewPro LRF state init after startup

rmackay9 · Mar 22, 2024 · 900c489 · 900c489
1 parent f030e05
commit 900c489
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git 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
@@ -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)
 };