AP_InertialSensor: return roll and pitch trim angles

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -311,7 +311,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
 // original sketch available at
 // http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
 bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
-                            AP_InertialSensor_UserInteract* interact)
+                            AP_InertialSensor_UserInteract* interact,
+                            float &trim_roll,
+                            float &trim_pitch)
 {
     Vector3f samples[6];
     Vector3f new_offsets;
@@ -382,6 +384,10 @@ bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
         _accel_offset.set(new_offsets);
         _accel_scale.set(new_scaling);
         _save_parameters();
+
+        // calculate the trims as well and pass back to caller
+        _calculate_trim(samples[0], trim_roll, trim_pitch);
+
         return true;
     }
 
@@ -541,5 +547,32 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
     }
 }
 
+// _calculate_trim  - calculates the x and y trim angles (in radians) given a raw accel sample (i.e. no scaling or offsets applied) taken when the vehicle was level
+void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch)
+{
+    // scale sample and apply offsets
+    Vector3f accel_scale = _accel_scale.get();
+    Vector3f accel_offsets = _accel_offset.get();
+    Vector3f scaled_accels_x( accel_sample.x * accel_scale.x - accel_offsets.x,
+                              0,
+                              accel_sample.z * accel_scale.z - accel_offsets.z );
+    Vector3f scaled_accels_y( 0,
+                              accel_sample.y * accel_scale.y - accel_offsets.y,
+                              accel_sample.z * accel_scale.z - accel_offsets.z );
+
+    // calculate x and y axis angle (i.e. roll and pitch angles)
+    Vector3f vertical = Vector3f(0,0,-1);
+    trim_roll = scaled_accels_y.angle(vertical);
+    trim_pitch = scaled_accels_x.angle(vertical);
+
+    // angle call doesn't return the sign so take care of it here
+    if( scaled_accels_y.y > 0 ) {
+        trim_roll = -trim_roll;
+    }
+    if( scaled_accels_x.x < 0 ) {
+        trim_pitch = -trim_pitch;
+    }
+}
+
 #endif // __AVR_ATmega1280__
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -64,7 +64,9 @@ public:
     // perform accelerometer calibration including providing user instructions
     // and feedback
     virtual bool calibrate_accel(void (*flash_leds_cb)(bool on),
-                                 AP_InertialSensor_UserInteract *interact);
+                                 AP_InertialSensor_UserInteract *interact,
+                                 float& trim_roll,
+                                 float& trim_pitch);
 #endif
 
     /// Perform cold-start initialisation for just the gyros.
@@ -144,6 +146,7 @@ protected:
     virtual void            _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
     virtual void            _calibrate_reset_matrices(float dS[6], float JS[6][6]);
     virtual void            _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
+    virtual void            _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
     // save parameters to eeprom