Copter: use ahrs trig values

ArduCopter/ArduCopter.pde

@@ -540,20 +540,6 @@ static int16_t control_pitch;           // desired pitch angle of copter in cent
 static uint8_t rtl_state;               // records state of rtl (initial climb, returning home, etc)
 static uint8_t land_state;              // records state of land (flying to location, descending)
 
-////////////////////////////////////////////////////////////////////////////////
-// Orientation
-////////////////////////////////////////////////////////////////////////////////
-// Convienience accessors for commonly used trig functions. These values are generated
-// by the DCM through a few simple equations. They are used throughout the code where cos and sin
-// would normally be used.
-// The cos values are defaulted to 1 to get a decent initial value for a level state
-static float cos_roll_x         = 1.0;
-static float cos_pitch_x        = 1.0;
-static float cos_yaw            = 1.0;
-static float sin_yaw;
-static float sin_roll;
-static float sin_pitch;
-
 ////////////////////////////////////////////////////////////////////////////////
 // SIMPLE Mode
 ////////////////////////////////////////////////////////////////////////////////
@@ -984,10 +970,6 @@ static void fast_loop()
     // --------------------
     read_AHRS();
 
-    // reads all of the necessary trig functions for cameras, throttle, etc.
-    // --------------------------------------------------------------------
-    update_trig();
-
 	// Acrobatic control
     if (ap.do_flip) {
         if(abs(g.rc_1.control_in) < 4000) {
@@ -1220,7 +1202,7 @@ static void update_optical_flow(void)
     // if new data has arrived, process it
     if( optflow.last_update != last_of_update ) {
         last_of_update = optflow.last_update;
-        optflow.update_position(ahrs.roll, ahrs.pitch, sin_yaw, cos_yaw, current_loc.alt);      // updates internal lon and lat with estimation based on optical flow
+        optflow.update_position(ahrs.roll, ahrs.pitch, ahrs.sin_yaw(), ahrs.cos_yaw(), current_loc.alt);      // updates internal lon and lat with estimation based on optical flow
 
         // write to log at 5hz
         of_log_counter++;
@@ -1617,13 +1599,13 @@ void update_roll_pitch_mode(void)
             g.rc_1.servo_out = g.rc_1.control_in;
             g.rc_2.servo_out = g.rc_2.control_in;
         }else{
-            acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
+            acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*ahrs.cos_pitch();
             get_roll_rate_stabilized_bf(g.rc_1.control_in);
             get_pitch_rate_stabilized_bf(g.rc_2.control_in);
             get_acro_level_rates();
         }
 #else  // !HELI_FRAME
-        acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
+        acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*ahrs.cos_pitch();
         get_roll_rate_stabilized_bf(g.rc_1.control_in);
         get_pitch_rate_stabilized_bf(g.rc_2.control_in);
         get_acro_level_rates();
@@ -1733,8 +1715,8 @@ static void
 init_simple_bearing()
 {
     // capture current cos_yaw and sin_yaw values
-    simple_cos_yaw = cos_yaw;
-    simple_sin_yaw = sin_yaw;
+    simple_cos_yaw = ahrs.cos_yaw();
+    simple_sin_yaw = ahrs.sin_yaw();
 
     // initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
     super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
@@ -1768,8 +1750,8 @@ void update_simple_mode(void)
     }
 
     // rotate roll, pitch input from north facing to vehicle's perspective
-    g.rc_1.control_in = rollx*cos_yaw + pitchx*sin_yaw;
-    g.rc_2.control_in = -rollx*sin_yaw + pitchx*cos_yaw;
+    g.rc_1.control_in = rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw();
+    g.rc_2.control_in = -rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw();
 }
 
 // update_super_simple_bearing - adjusts simple bearing based on location
@@ -2031,39 +2013,6 @@ static void read_AHRS(void)
     omega = ins.get_gyro();
 }
 
-static void update_trig(void){
-    Vector2f yawvector;
-    const Matrix3f &temp   = ahrs.get_dcm_matrix();
-
-    yawvector.x     = temp.a.x;     // sin
-    yawvector.y     = temp.b.x;         // cos
-    yawvector.normalize();
-
-    cos_pitch_x     = safe_sqrt(1 - (temp.c.x * temp.c.x));     // level = 1
-    cos_roll_x      = temp.c.z / cos_pitch_x;                       // level = 1
-
-    cos_pitch_x     = constrain_float(cos_pitch_x, 0, 1.0);
-    // this relies on constrain_float() of infinity doing the right thing,
-    // which it does do in avr-libc
-    cos_roll_x      = constrain_float(cos_roll_x, -1.0, 1.0);
-
-    sin_yaw         = constrain_float(yawvector.y, -1.0, 1.0);
-    cos_yaw         = constrain_float(yawvector.x, -1.0, 1.0);
-
-    // added to convert earth frame to body frame for rate controllers
-    sin_pitch       = -temp.c.x;
-    sin_roll        = temp.c.y / cos_pitch_x;
-
-    // update wp_nav controller with trig values
-    wp_nav.set_cos_sin_yaw(cos_yaw, sin_yaw, cos_pitch_x);
-
-    //flat:
-    // 0 ° = cos_yaw:  1.00, sin_yaw:  0.00,
-    // 90° = cos_yaw:  0.00, sin_yaw:  1.00,
-    // 180 = cos_yaw: -1.00, sin_yaw:  0.00,
-    // 270 = cos_yaw:  0.00, sin_yaw: -1.00,
-}
-
 // read baro and sonar altitude at 20hz
 static void update_altitude()
 {

ArduCopter/Attitude.pde

@@ -432,14 +432,14 @@ update_rate_contoller_targets()
 {
     if( rate_targets_frame == EARTH_FRAME ) {
         // convert earth frame rates to body frame rates
-        roll_rate_target_bf     = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
-        pitch_rate_target_bf    = cos_roll_x  * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
-        yaw_rate_target_bf      = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
+        roll_rate_target_bf     = roll_rate_target_ef - ahrs.sin_pitch() * yaw_rate_target_ef;
+        pitch_rate_target_bf    = ahrs.cos_roll()  * pitch_rate_target_ef + ahrs.sin_roll() * ahrs.cos_pitch() * yaw_rate_target_ef;
+        yaw_rate_target_bf      = ahrs.cos_pitch() * ahrs.cos_roll() * yaw_rate_target_ef - ahrs.sin_roll() * pitch_rate_target_ef;
     }else if( rate_targets_frame == BODY_EARTH_FRAME ) {
         // add converted earth frame rates to body frame rates
-        acro_roll_rate = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
-        acro_pitch_rate = cos_roll_x  * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
-        acro_yaw_rate = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
+        acro_roll_rate = roll_rate_target_ef - ahrs.sin_pitch() * yaw_rate_target_ef;
+        acro_pitch_rate = ahrs.cos_roll() * pitch_rate_target_ef + ahrs.sin_roll() * ahrs.cos_pitch() * yaw_rate_target_ef;
+        acro_yaw_rate = ahrs.cos_pitch() * ahrs.cos_roll() * yaw_rate_target_ef - ahrs.sin_roll() * pitch_rate_target_ef;
     }
 }
 
@@ -738,7 +738,7 @@ static void update_throttle_cruise(int16_t throttle)
 // for traditional helicopters
 static int16_t get_angle_boost(int16_t throttle)
 {
-    float angle_boost_factor = cos_pitch_x * cos_roll_x;
+    float angle_boost_factor = ahrs.cos_pitch() * ahrs.cos_roll();
     angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f);
     int16_t throttle_above_mid = max(throttle - motors.get_collective_mid(),0);
 
@@ -752,7 +752,7 @@ static int16_t get_angle_boost(int16_t throttle)
 // throttle value should be 0 ~ 1000
 static int16_t get_angle_boost(int16_t throttle)
 {
-    float temp = cos_pitch_x * cos_roll_x;
+    float temp = ahrs.cos_pitch() * ahrs.cos_roll();
     int16_t throttle_out;
 
     temp = constrain_float(temp, 0.5f, 1.0f);

ArduCopter/drift.pde

@@ -27,8 +27,8 @@ get_yaw_drift()
     Vector3f vel = inertial_nav.get_velocity();
 
     // rotate roll, pitch input from north facing to vehicle's perspective
-    float roll_vel =  vel.y * cos_yaw - vel.x * sin_yaw; // body roll vel
-    float pitch_vel = vel.y * sin_yaw + vel.x * cos_yaw; // body pitch vel
+    float roll_vel =  vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
+    float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
 
     float pitch_vel2 = min(fabs(pitch_vel), 800);
     // simple gain scheduling for yaw input

ArduCopter/navigation.pde

@@ -197,8 +197,8 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
     float cir_radius = g.circle_radius * 100;
 
     // set circle center to circle_radius ahead of current position
-    circle_center.x = current_position.x + cir_radius * cos_yaw;
-    circle_center.y = current_position.y + cir_radius * sin_yaw;
+    circle_center.x = current_position.x + cir_radius * ahrs.cos_yaw();
+    circle_center.y = current_position.y + cir_radius * ahrs.sin_yaw();
 
     // if we are doing a panorama set the circle_angle to the current heading
     if( g.circle_radius <= 0 ) {

ArduCopter/sensors.pde

@@ -54,7 +54,7 @@ static int16_t read_sonar(void)
 
  #if SONAR_TILT_CORRECTION == 1
     // correct alt for angle of the sonar
-    float temp = cos_pitch_x * cos_roll_x;
+    float temp = ahrs.cos_pitch() * ahrs.cos_roll();
     temp = max(temp, 0.707f);
     temp_alt = (float)temp_alt * temp;
  #endif