Merge pull request #53 from larus-breeze/add_fine_tune_orientation

Add fine tune sensor orientation

Author: realtimepeople

Generic_Algorithms/quaternion.h

@@ -78,28 +78,6 @@ template <class datatype > class quaternion: public vector <datatype, 4>
 		return _euler;
 	}
 
-	//! quaternion chaining, multiplication
-	quaternion<datatype> operator *( const quaternion<datatype> right) const
-	{
-	  quaternion<datatype> result;
-	  datatype w1 = vector<datatype, 4>::e[0];
-	  datatype x1 = vector<datatype, 4>::e[1];
-	  datatype y1 = vector<datatype, 4>::e[2];
-	  datatype z1 = vector<datatype, 4>::e[3];
-
-	  datatype w2 = right[0];
-	  datatype x2 = right[1];
-	  datatype y2 = right[2];
-	  datatype z2 = right[3];
-
-	  result[0] = w1*w2 - x1*x2 - y1*y2 - z1*z2;
-	  result[1] = w1*x2 + x1*w2 + y1*z2 - z1*y2;
-	  result[2] = w1*y2 - x1*z2 + y1*w2 + z1*x2;
-	  result[3] = w1*z2 + x1*y2 - y1*x2 + z1*w2;
-
-	  return result;
-	}
-
 	//! get north component of attitude
 	inline datatype get_north( void) const
 	{
@@ -208,13 +186,19 @@ template <class datatype > class quaternion: public vector <datatype, 4>
 		datatype re1=right.e[1];
 		datatype re2=right.e[2];
 		datatype re3=right.e[3];
-		result.e[0] = e0 * re0 - e1 * re1 - e2 * re2 + e3 * re3;
+		result.e[0] = e0 * re0 - e1 * re1 - e2 * re2 - e3 * re3;
 		result.e[1] = e1 * re0 + e2 * re3 - e3 * re2 + e0 * re1;
 		result.e[2] = e3 * re1 + e0 * re2 - e1 * re3 + e2 * re0;
 		result.e[3] = e1 * re2 - e2 * re1 + e0 * re3 + e3 * re0;
 		return result;
 		}
 
+	quaternion <datatype> operator *= ( const quaternion <datatype> & right) //!< quaternion multiplication
+		{
+		  *this = *this * right;
+		  return *this;
+		}
+
 	void from_rotation_matrix( matrix<datatype, 3> &rotm) //!< rotation matrix -> quaternion transformation
 		{
 		float tmp;
@@ -229,5 +213,16 @@ template <class datatype > class quaternion: public vector <datatype, 4>
 		this->e[3] = tmp * (rotm.e[1][0] - rotm.e[0][1]);
 		normalize(); // compensate computational inaccuracies
 		};
+
+	quaternion <datatype> inverse( void)
+	{
+	  quaternion <datatype> retv;
+	  retv[0] = this->e[0];
+	  retv[1] = -this->e[1];
+	  retv[2] = -this->e[2];
+	  retv[3] = -this->e[3];
+	  retv.normalize();
+	  return retv;
+	}
 };
 #endif // QUATERNION_H

NAV_Algorithms/organizer.h

@@ -64,20 +64,21 @@ class organizer_t
     u_front_sensor.normalize();
 
     // calculate the new rotation matrix using our calibration data
-    sensor_mapping.e[0][0]=u_front_sensor[0];
-    sensor_mapping.e[0][1]=u_front_sensor[1];
-    sensor_mapping.e[0][2]=u_front_sensor[2];
+    float3matrix new_sensor_mapping;
+    new_sensor_mapping.e[0][0]=u_front_sensor[0];
+    new_sensor_mapping.e[0][1]=u_front_sensor[1];
+    new_sensor_mapping.e[0][2]=u_front_sensor[2];
 
-    sensor_mapping.e[1][0]=u_right_sensor[0];
-    sensor_mapping.e[1][1]=u_right_sensor[1];
-    sensor_mapping.e[1][2]=u_right_sensor[2];
+    new_sensor_mapping.e[1][0]=u_right_sensor[0];
+    new_sensor_mapping.e[1][1]=u_right_sensor[1];
+    new_sensor_mapping.e[1][2]=u_right_sensor[2];
 
-    sensor_mapping.e[2][0]=u_down_sensor[0];
-    sensor_mapping.e[2][1]=u_down_sensor[1];
-    sensor_mapping.e[2][2]=u_down_sensor[2];
+    new_sensor_mapping.e[2][0]=u_down_sensor[0];
+    new_sensor_mapping.e[2][1]=u_down_sensor[1];
+    new_sensor_mapping.e[2][2]=u_down_sensor[2];
 
     quaternion<float> q;
-    q.from_rotation_matrix( sensor_mapping);
+    q.from_rotation_matrix( new_sensor_mapping);
     eulerangle<float> euler = q;
 
     // make the change permanent
@@ -88,6 +89,34 @@ class organizer_t
     lock_EEPROM( true);
   }
 
+  void fine_tune_sensor_orientation( const vector_average_collection_t & values)
+  {
+    float3vector observed_body_acc  = sensor_mapping * values.acc_observed_level;
+    float pitch_correction = - ATAN2( - observed_body_acc[FRONT], - observed_body_acc[DOWN]);
+    float roll_correction  = + ATAN2( - observed_body_acc[RIGHT], - observed_body_acc[DOWN]);
+
+    quaternion<float> q_correction;
+    q_correction.from_euler( roll_correction, pitch_correction, ZERO);
+
+    quaternion<float> q_present_setting;
+    q_present_setting.from_euler (
+	configuration (SENS_TILT_ROLL),
+	configuration (SENS_TILT_PITCH),
+	configuration (SENS_TILT_YAW));
+
+    quaternion<float> q_new_setting;
+    q_new_setting = q_correction * q_present_setting;
+
+    eulerangle<float> new_euler = q_new_setting;
+
+    // make the change permanent
+    lock_EEPROM( false);
+    write_EEPROM_value( SENS_TILT_ROLL,  new_euler.roll);
+    write_EEPROM_value( SENS_TILT_PITCH, new_euler.pitch);
+    write_EEPROM_value( SENS_TILT_YAW,   new_euler.yaw);
+    lock_EEPROM( true);
+  }
+
   void initialize_before_measurement( void)
   {
     pitot_offset= configuration (PITOT_OFFSET);

NAV_Algorithms/setup_tester.cpp

@@ -1,6 +1,7 @@
 #include "float3vector.h"
 #include "float3matrix.h"
 #include "quaternion.h"
+#include "AHRS.h"
 
 const float ldi[]={0.1f, +0.25f, -10.0f}; // left wing down
 const float rdi[]={0.1f, -0.15f, -10.0f}; // right wing down
@@ -18,73 +19,122 @@ void setup_tester(void)
   float3vector horiz_body( hi);
 
   // setup the sensor orientation to test
-  quaternion<float>sensor_q;
+  quaternion<float>q_sensor_2_body;
+//  q_sensor_2_body.from_euler( 0 * M_PI/180.0, 0 * M_PI/180.0, 13 * M_PI/180.0);
+  q_sensor_2_body.from_euler( 90 * M_PI/180.0, 180.0 * M_PI/180.0, -30 * M_PI/180.0);
 //  sensor_q.from_euler( 45.0 * M_PI/180.0, 135.0 * M_PI/180.0, -30.0 * M_PI/180.0);
 //  sensor_q.from_euler( -135.0 * M_PI/180.0, 45.0 * M_PI/180.0, 150.0 * M_PI/180.0);
 //  sensor_q.from_euler( -10.0 * M_PI/180.0, 75.0 * M_PI/180.0, -45.0 * M_PI/180.0);
-  sensor_q.from_euler( 10.0 * M_PI/180.0, -75.0 * M_PI/180.0, +125.0 * M_PI/180.0);
+//  sensor_q.from_euler( 10.0 * M_PI/180.0, -75.0 * M_PI/180.0, +125.0 * M_PI/180.0);
 
-  float3matrix sensor_2_body_assumption;
-  sensor_q.get_rotation_matrix(sensor_2_body_assumption);
+  float3matrix m_sensor_2_body_assumption;
+  q_sensor_2_body.get_rotation_matrix( m_sensor_2_body_assumption);
 
   //check if we get the rotation angles using the matrix given
-  quaternion<float> qin;
-  qin.from_rotation_matrix(sensor_2_body_assumption);
-  eulerangle<float> euler_in = qin;
+  quaternion<float> q_sensor_2_body_assumption;
+  q_sensor_2_body_assumption.from_rotation_matrix(m_sensor_2_body_assumption);
+  eulerangle<float> euler_in = q_sensor_2_body_assumption;
   euler_in.roll  *= 180/M_PI;
   euler_in.pitch *= 180/M_PI;
   euler_in.yaw   *= 180/M_PI;
 
   // map measurements into the sensor frame
-  float3vector left_down_sensor	= sensor_2_body_assumption.reverse_map( left_down_body);
-  float3vector right_down_sensor= sensor_2_body_assumption.reverse_map( right_down_body);
-  float3vector horiz_sensor 	= sensor_2_body_assumption.reverse_map( horiz_body);
+  float3vector v_left_down_sensor	= m_sensor_2_body_assumption.reverse_map( left_down_body);
+  float3vector v_right_down_sensor= m_sensor_2_body_assumption.reverse_map( right_down_body);
+  float3vector v_horiz_sensor 	= m_sensor_2_body_assumption.reverse_map( horiz_body);
 
   // resolve sensor orientation using measurements
-  float3vector front_down_sensor_helper = right_down_sensor.vector_multiply(left_down_sensor);
-  float3vector u_right_sensor = front_down_sensor_helper.vector_multiply(horiz_sensor);
+  float3vector front_down_sensor_helper = v_right_down_sensor.vector_multiply(v_left_down_sensor);
+  float3vector u_right_sensor = front_down_sensor_helper.vector_multiply(v_horiz_sensor);
   u_right_sensor.normalize();
 
-  float3vector u_down_sensor = horiz_sensor * -1.0f;
+  float3vector u_down_sensor = v_horiz_sensor * -1.0f;
   u_down_sensor.normalize();
 
   float3vector u_front_sensor=u_right_sensor.vector_multiply(u_down_sensor);
   u_front_sensor.normalize();
 
   // calculate the rotation matrix using our calibration data
-  float3matrix sensor_2_body;
-  sensor_2_body.e[0][0]=u_front_sensor[0];
-  sensor_2_body.e[0][1]=u_front_sensor[1];
-  sensor_2_body.e[0][2]=u_front_sensor[2];
+  float3matrix m_sensor_2_body;
+  m_sensor_2_body.e[FRONT][0]=u_front_sensor[0];
+  m_sensor_2_body.e[FRONT][1]=u_front_sensor[1];
+  m_sensor_2_body.e[FRONT][2]=u_front_sensor[2];
 
-  sensor_2_body.e[1][0]=u_right_sensor[0];
-  sensor_2_body.e[1][1]=u_right_sensor[1];
-  sensor_2_body.e[1][2]=u_right_sensor[2];
+  m_sensor_2_body.e[RIGHT][0]=u_right_sensor[0];
+  m_sensor_2_body.e[RIGHT][1]=u_right_sensor[1];
+  m_sensor_2_body.e[RIGHT][2]=u_right_sensor[2];
 
-  sensor_2_body.e[2][0]=u_down_sensor[0];
-  sensor_2_body.e[2][1]=u_down_sensor[1];
-  sensor_2_body.e[2][2]=u_down_sensor[2];
+  m_sensor_2_body.e[DOWN][0]=u_down_sensor[0];
+  m_sensor_2_body.e[DOWN][1]=u_down_sensor[1];
+  m_sensor_2_body.e[DOWN][2]=u_down_sensor[2];
 
   // test if the rotation matrix recovers the body frame data
-  float3vector test_left_down_body = sensor_2_body * left_down_sensor;
-  float3vector test_right_down_body = sensor_2_body * right_down_sensor;
-  float3vector test_horiz_body = sensor_2_body * horiz_sensor;
+  float3vector test_left_down_body = m_sensor_2_body * v_left_down_sensor;
+  float3vector test_right_down_body = m_sensor_2_body * v_right_down_sensor;
+  float3vector test_horiz_body = m_sensor_2_body * v_horiz_sensor;
 
   // test if the matrix provides pure rotation
-  float3vector vtest1 = sensor_2_body * float3vector( test1);
+  float3vector vtest1 = m_sensor_2_body * float3vector( test1);
   float test1_abs=vtest1.abs();
-  float3vector vtest2 = sensor_2_body * float3vector( test2);
+  float3vector vtest2 = m_sensor_2_body * float3vector( test2);
   float test2_abs=vtest2.abs();
-  float3vector vtest3 = sensor_2_body * float3vector( test3);
+  float3vector vtest3 = m_sensor_2_body * float3vector( test3);
   float test3_abs=vtest3.abs();
 
   //check if we get the rotation angles using the matrix we have calculated
   quaternion<float> q;
-  q.from_rotation_matrix(sensor_2_body);
+  q.from_rotation_matrix(m_sensor_2_body);
   eulerangle<float> euler = q;
   euler.roll  *= 180/M_PI;
   euler.pitch *= 180/M_PI;
   euler.yaw   *= 180/M_PI;
+
+  ////////////////////////////////////////////////////////////////////////////////////////////////
+
+  // pitch and roll axis fine tuning
+  float pitch_angle = 0.5f; 	// sensor nose is higher than configured
+  float roll_angle =  -0.3f; 	// sensor is looking more left than configured
+  quaternion<float>q_sensor_ideal_2_misaligned;
+  q_sensor_ideal_2_misaligned.from_euler( roll_angle, pitch_angle, ZERO);
+
+  quaternion<float>q_body_2_world; // looking NE horizontally
+  q_body_2_world.from_euler( ZERO, ZERO, 45.0f * M_PI_F / 180.0f);
+//  q_body_2_world.from_euler( ZERO, ZERO, -145.0f * M_PI_F / 180.0f);
+
+  // provide simulated measurement
+  quaternion <float> q_misaligned_sensor_2_world = q_sensor_ideal_2_misaligned;
+  q_misaligned_sensor_2_world *= q_sensor_2_body;
+  q_misaligned_sensor_2_world *= q_body_2_world;
+
+  euler = q_misaligned_sensor_2_world;
+  euler.roll  *= 180/M_PI;
+  euler.pitch *= 180/M_PI;
+  euler.yaw   *= 180/M_PI;
+
+  float3matrix m_misaligned_sensor_to_world;
+  q_misaligned_sensor_2_world.get_rotation_matrix( m_misaligned_sensor_to_world);
+
+  // simulate measurement
+  float3vector v_down;
+  v_down[DOWN] = -10.0f;
+  float3vector v_measurement_sensor = m_misaligned_sensor_to_world.reverse_map( v_down);
+
+  q_sensor_2_body.get_rotation_matrix( m_sensor_2_body);
+
+  float3vector v_measurement_body = m_sensor_2_body * v_measurement_sensor;
+
+  float3matrix m_body_2_world;
+  q_body_2_world.get_rotation_matrix(m_body_2_world);
+  float3vector v_measurement_world = m_body_2_world * v_measurement_body;
+
+  float pitch_angle_measured = ATAN2( -v_measurement_world[0], -v_measurement_world[2]);
+  float roll_angle_measured = ATAN2( -v_measurement_world[1], -v_measurement_world[2]);
+
+  // compute corrected sensor alignment quaternion
+  quaternion <float> q_correction;
+  q_correction.from_euler( roll_angle_measured, pitch_angle_measured, ZERO);
+
+  quaternion<float> q_corrected_sensor_orientation;
 }