AP_AHRS: call INS update in AP_AHRS::update w/o holding data sem

We shouldn't be holding the data semaphore while waiting for the samples
- lots of things might be unnecessarily blocked

DCM's update function doesn't need to take the semaphore as it is
already taken by AP_AHRS

libraries/AP_AHRS/AP_AHRS.cpp

@@ -30,6 +30,7 @@
 #include <AP_Notify/AP_Notify.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 
 #define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
 #define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
@@ -287,6 +288,11 @@ void AP_AHRS::reset_gyro_drift(void)
 
 void AP_AHRS::update(bool skip_ins_update)
 {
+    if (!skip_ins_update) {
+        // tell the IMU to grab some data
+        AP::ins().update();
+    }
+
     // support locked access functions to AHRS data
     WITH_SEMAPHORE(_rsem);
 
@@ -303,7 +309,7 @@ void AP_AHRS::update(bool skip_ins_update)
     // update autopilot-body-to-vehicle-body from _trim parameters:
     update_trim_rotation_matrices();
 
-    update_DCM(skip_ins_update);
+    update_DCM();
 
     // update takeoff/touchdown flags
     update_flags();
@@ -348,7 +354,7 @@ void AP_AHRS::update(bool skip_ins_update)
 
     if (_view != nullptr) {
         // update optional alternative attitude view
-        _view->update(skip_ins_update);
+        _view->update();
     }
 
     // update AOA and SSA
@@ -394,7 +400,7 @@ void AP_AHRS::update(bool skip_ins_update)
     }
 }
 
-void AP_AHRS::update_DCM(bool skip_ins_update)
+void AP_AHRS::update_DCM()
 {
     // we need to restore the old DCM attitude values as these are
     // used internally in DCM to calculate error values for gyro drift
@@ -404,7 +410,7 @@ void AP_AHRS::update_DCM(bool skip_ins_update)
     yaw = _dcm_attitude.z;
     update_cd_values();
 
-    AP_AHRS_DCM::update(skip_ins_update);
+    AP_AHRS_DCM::update();
 
     // keep DCM attitude available for get_secondary_attitude()
     _dcm_attitude = {roll, pitch, yaw};

libraries/AP_AHRS/AP_AHRS.h

@@ -90,7 +90,7 @@ public:
     //  should be called if gyro offsets are recalculated
     void reset_gyro_drift() override;
 
-    void            update(bool skip_ins_update=false) override;
+    void            update(bool skip_ins_update=false);
     void            reset(bool recover_eulers = false) override;
 
     // dead-reckoning support
@@ -532,7 +532,7 @@ private:
     uint8_t _ekf_flags; // bitmask from Flags enumeration
 
     EKFType ekf_type(void) const;
-    void update_DCM(bool skip_ins_update);
+    void update_DCM();
 
     // get the index of the current primary IMU
     uint8_t get_primary_IMU_index(void) const;

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -78,7 +78,7 @@ public:
     }
 
     // Methods
-    virtual void update(bool skip_ins_update=false) = 0;
+    virtual void update() = 0;
 
     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     // requires_position should be true if horizontal position configuration should be checked

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -65,18 +65,10 @@ void AP_AHRS::load_watchdog_home()
 
 // run a full DCM update round
 void
-AP_AHRS_DCM::update(bool skip_ins_update)
+AP_AHRS_DCM::update()
 {
-    // support locked access functions to AHRS data
-    WITH_SEMAPHORE(_rsem);
-
     AP_InertialSensor &_ins = AP::ins();
 
-    if (!skip_ins_update) {
-        // tell the IMU to grab some data
-        _ins.update();
-    }
-
     // ask the IMU how much time this sensor reading represents
     const float delta_t = _ins.get_delta_time();
 

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -58,7 +58,7 @@ public:
     void reset_gyro_drift() override;
 
     // Methods
-    void            update(bool skip_ins_update=false) override;
+    void            update() override;
     void            reset(bool recover_eulers = false) override;
 
     // return true if yaw has been initialised

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -58,7 +58,7 @@ void AP_AHRS_View::set_pitch_trim(float trim_deg) {
 };
 
 // update state
-void AP_AHRS_View::update(bool skip_ins_update)
+void AP_AHRS_View::update()
 {
     rot_body_to_ned = ahrs.get_rotation_body_to_ned();
     gyro = ahrs.get_gyro();

libraries/AP_AHRS/AP_AHRS_View.h

@@ -34,7 +34,7 @@ public:
     AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
 
     // update state
-    void update(bool skip_ins_update=false);
+    void update();
 
     // empty virtual destructor
     virtual ~AP_AHRS_View() {}