AP_AHRS: added attitude_reset() method for HIL_SENSORS

libraries/AP_AHRS/AP_AHRS.h

@@ -117,6 +117,9 @@ public:
     // reset the current attitude, used on new IMU calibration
     virtual void reset(bool recover_eulers=false) = 0;
 
+    // reset the current attitude, used on new IMU calibration
+    virtual void reset_attitude(const float &roll, const float &pitch, const float &yaw) = 0;
+
     // how often our attitude representation has gone out of range
     uint8_t renorm_range_count;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -117,6 +117,12 @@ AP_AHRS_DCM::reset(bool recover_eulers)
     }
 }
 
+// reset the current attitude, used by HIL
+void AP_AHRS_DCM::reset_attitude(const float &roll, const float &pitch, const float &yaw)
+{
+    _dcm_matrix.from_euler(roll, pitch, yaw);    
+}
+
 /*
  *  check the DCM matrix for pathological values
  */

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -55,6 +55,9 @@ public:
     void            update(void);
     void            reset(bool recover_eulers = false);
 
+    // reset the current attitude, used on new IMU calibration
+    void reset_attitude(const float &roll, const float &pitch, const float &yaw);
+
     // dead-reckoning support
     bool get_position(struct Location &loc);
 

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -36,6 +36,10 @@ public:
     void reset(bool recover_eulers=false) {
     }
 
+    void reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) {
+        // not implemented - use setHil()
+    }
+
     float get_error_rp(void) {
         return 0;
     }