AP_AHRS: update rotation based on board_orient parameter

libraries/AP_AHRS/AP_AHRS.cpp

@@ -304,6 +304,12 @@ void AP_AHRS::reset_gyro_drift(void)
 
 void AP_AHRS::update(bool skip_ins_update)
 {
+    // periodically checks to see if we should update the AHRS
+    // orientation (e.g. based on the AHRS_ORIENTATION parameter)
+    // allow for runtime change of orientation
+    // this makes initial config easier
+    update_orientation();
+
     if (!skip_ins_update) {
         // tell the IMU to grab some data
         AP::ins().update();

libraries/AP_AHRS/AP_AHRS.h

@@ -79,6 +79,8 @@ public:
         return _singleton;
     }
 
+    // periodically checks to see if we should update the AHRS
+    // orientation (e.g. based on the AHRS_ORIENTATION parameter)
     // allow for runtime change of orientation
     // this makes initial config easier
     void update_orientation();
@@ -774,6 +776,9 @@ private:
     Matrix3f _rotation_autopilot_body_to_vehicle_body;
     Matrix3f _rotation_vehicle_body_to_autopilot_body;
 
+    // last time orientation was updated from AHRS_ORIENTATION:
+    uint32_t last_orientation_update_ms;
+
     // updates matrices responsible for rotating vectors from vehicle body
     // frame to autopilot body frame from _trim variables
     void update_trim_rotation_matrices();

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -66,10 +66,25 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
     }
 }
 
-// Set the board mounting orientation, may be called while disarmed
+// Set the board mounting orientation from AHRS_ORIENTATION parameter
 void AP_AHRS::update_orientation()
 {
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_orientation_update_ms < 1000) {
+        // only update once/second
+        return;
+    }
+
+    // never update while armed - unless we've never updated
+    // (e.g. mid-air reboot or ARMING_REQUIRED=NO on Plane):
+    if (hal.util->get_soft_armed() && last_orientation_update_ms != 0) {
+        return;
+    }
+
+    last_orientation_update_ms = now_ms;
+
     const enum Rotation orientation = (enum Rotation)_board_orientation.get();
+
     AP::ins().set_board_orientation(orientation);
     AP::compass().set_board_orientation(orientation);
 }