AP_Gimbal: remove gyro bias saving feature

3drobotics · Sep 30, 2015 · 282b8fc · 282b8fc
1 parent 19898e0
commit 282b8fc
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 47 deletions.
diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
@@ -65,8 +65,6 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
             _gimbalParams.update();
             if (_gimbalParams.initialized()) {
                 // parameters done initializing, finalize initialization and transition to aligning
-                _initial_gyro_bias = _gimbalParams.get_gyro_bias();
-                _expected_gyro_bias = _initial_gyro_bias;
                 extract_feedback(report_msg);
                 _ang_vel_mag_filt = 20;
                 filtered_joint_angles = _measurement.joint_angles;
@@ -83,15 +81,13 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
             if (_ekf.getStatus()) {
                 // EKF done aligning, transition to running
                 _state = GIMBAL_STATE_PRESENT_RUNNING;
-                _last_gyro_bias_save_ms = hal.scheduler->millis();
             }
             break;
 
         case GIMBAL_STATE_PRESENT_RUNNING:
             _gimbalParams.update();
             extract_feedback(report_msg);
             update_estimators();
-            update_gimbal_gyro_bias();
             break;
     }
 
@@ -160,44 +156,6 @@ void AP_Gimbal::send_controls(mavlink_channel_t chan)
     }
 }
 
-void AP_Gimbal::update_gimbal_gyro_bias()
-{
-    // reset the timer if EKF is not aligned
-    if (!_ekf.getStatus()) {
-        _last_gyro_bias_save_ms = hal.scheduler->millis();
-        return;
-    }
-
-    // wait 60 seconds before updating gyro bias. flashing blocks momentarily, so don't do it while armed
-    if (hal.util->get_soft_armed() || hal.scheduler->millis()-_last_gyro_bias_save_ms < 60000) {
-        return;
-    }
-
-    _last_gyro_bias_save_ms = hal.scheduler->millis();
-
-    Vector3f gimbal_gyro_bias = _gimbalParams.get_gyro_bias();
-
-    // if there was an external modification to the gyro bias parameter, don't save
-    if (_expected_gyro_bias != gimbal_gyro_bias) {
-        return;
-    }
-
-    Vector3f total_gyro_bias;
-    _ekf.getGyroBias(total_gyro_bias);
-    total_gyro_bias += gimbal_gyro_bias;
-
-    // allow a maximum of 1 deg/s of change per boot cycle
-    Vector3f output_gyro_bias = total_gyro_bias;
-    output_gyro_bias.x = constrain_float(output_gyro_bias.x, _initial_gyro_bias.x-radians(1.0f), _initial_gyro_bias.x+radians(1.0f));
-    output_gyro_bias.y = constrain_float(output_gyro_bias.y, _initial_gyro_bias.y-radians(1.0f), _initial_gyro_bias.y+radians(1.0f));
-    output_gyro_bias.z = constrain_float(output_gyro_bias.z, _initial_gyro_bias.z-radians(1.0f), _initial_gyro_bias.z+radians(1.0f));
-
-    _expected_gyro_bias = output_gyro_bias;
-    _gimbalParams.set_gyro_bias(output_gyro_bias);
-    _ekf.setGyroBias(total_gyro_bias - output_gyro_bias);
-    _gimbalParams.flash();
-}
-
 void AP_Gimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg)
 {
     _measurement.delta_time = report_msg.delta_time;

diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h
@@ -107,7 +107,6 @@ class AP_Gimbal : AP_AccelCal_Client
     void send_controls(mavlink_channel_t chan);
     void extract_feedback(const mavlink_gimbal_report_t& report_msg);
     void update_joint_angle_est();
-    void update_gimbal_gyro_bias();
 
     gimbal_mode_t get_mode();
 
@@ -138,10 +137,6 @@ class AP_Gimbal : AP_AccelCal_Client
 
     float _ang_vel_mag_filt;
 
-    uint32_t _last_gyro_bias_save_ms;
-    Vector3f _initial_gyro_bias;
-    Vector3f _expected_gyro_bias;
-
     AccelCalibrator _calibrator;
     void _acal_save_calibrations();
     bool _acal_ready_to_sample();