diff --git a/src/laser_diode/laser_diode.rs b/src/laser_diode/laser_diode.rs
index 3a4c047..57bd8e4 100644
--- a/src/laser_diode/laser_diode.rs
+++ b/src/laser_diode/laser_diode.rs
@@ -1,16 +1,19 @@
 use miniconf::Miniconf;
 use stm32f4xx_hal::pac::ADC2;
+use uom::si::electric_current::ampere;
 use crate::laser_diode::ld_ctrl::LdCtrl;
 use crate::laser_diode::ld_pwr_exc_protector::{LdPwrExcProtector, self};
 use crate::laser_diode::pd_responsitivity;
 use core::marker::PhantomData;
+use crate::device::sys_timer::sleep;
 
 use uom::si::{
     electric_current::milliampere,
     f64::{ElectricPotential, ElectricCurrent, Power},
 };
+use num_traits::Float;
 
-use uom::{si::{ISQ, SI, Quantity}, typenum::*};
+use uom::{si::{ISQ, SI, Quantity, ratio::ratio}, typenum::*};
 
 // Volt / Ampere
 pub type TransimpedanceUnit = Quantity<ISQ<P2, P1, N3, N2, Z0, Z0, Z0>, SI<f64>, f64>;
@@ -36,6 +39,14 @@ impl Settings{
         units: PhantomData,
         value: 10.0 / 0.75,
     };
+
+    const LD_CURRENT_STEP_SIZE: ElectricCurrent = ElectricCurrent {
+        dimension: PhantomData,
+        units: PhantomData,
+        value: 0.0001,
+    };
+
+    const LD_CURRENT_TIME_STEP_MS: u32 = 1;
 }
 
 #[derive(Clone, Debug, Miniconf)]
@@ -105,11 +116,28 @@ impl LdDrive{
         LdPwrExcProtector::get_status().v * Settings::PD_MON_TRANSCONDUCTANCE
     }
 
+    // Ramping up or down laser diode current according to preset current step size and time step.
     pub fn ld_set_i(&mut self, i: ElectricCurrent) -> ElectricCurrent {
-        let ld_i_set = i.min(self.settings.ld_drive_current_limit);
-        let ld_i_set = self.ctrl.set_i(ld_i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX);
-        self.settings.ld_drive_current = ld_i_set;
-        ld_i_set
+        let mut prev_i_set = self.settings.ld_drive_current;
+        let final_i_set = i.min(self.settings.ld_drive_current_limit).max(ElectricCurrent::new::<ampere>(0.0));
+        
+        let num_of_step = ((final_i_set - prev_i_set)/Settings::LD_CURRENT_STEP_SIZE).get::<ratio>().floor() as i32;
+
+        let current_step = if num_of_step.is_positive() {
+            Settings::LD_CURRENT_STEP_SIZE
+        } else { 
+            -Settings::LD_CURRENT_STEP_SIZE 
+        };
+        
+        for _ in (0..num_of_step.abs()).rev() {
+            prev_i_set = prev_i_set + current_step;
+            let _ = self.ctrl.set_i(prev_i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX);
+            sleep(Settings::LD_CURRENT_TIME_STEP_MS);
+        }
+        let prev_i_set = self.ctrl.set_i(final_i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX);
+
+        self.settings.ld_drive_current = prev_i_set;
+        prev_i_set
     }
 
     // Set the calibrated VDDA value obtained from ADC1 calibration