Copter: use common angle wrap code

ArduCopter/ArduCopter.pde

@@ -1646,8 +1646,8 @@ void update_roll_pitch_mode(void)
         control_pitch = g.rc_2.control_in;
 
         // copy latest output from nav controller to stabilize controller
-        nav_roll    += constrain_int32(wrap_180(auto_roll  - nav_roll),  -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
-        nav_pitch   += constrain_int32(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
+        nav_roll    += constrain_int32(wrap_180_cd(auto_roll  - nav_roll),  -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
+        nav_pitch   += constrain_int32(wrap_180_cd(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
         get_stabilize_roll(nav_roll);
         get_stabilize_pitch(nav_pitch);
 
@@ -1691,8 +1691,8 @@ void update_roll_pitch_mode(void)
         }
 
         // copy latest output from nav controller to stabilize controller
-        nav_roll    += constrain_int32(wrap_180(auto_roll  - nav_roll),  -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
-        nav_pitch   += constrain_int32(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
+        nav_roll    += constrain_int32(wrap_180_cd(auto_roll  - nav_roll),  -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
+        nav_pitch   += constrain_int32(wrap_180_cd(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second
         get_stabilize_roll(nav_roll);
         get_stabilize_pitch(nav_pitch);
         break;
@@ -1721,7 +1721,7 @@ void update_simple_mode(void)
     // which improves speed of function
     simple_counter++;
 
-    int16_t delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100;
+    int16_t delta = wrap_360_cd(ahrs.yaw_sensor - initial_simple_bearing)/100;
 
     if (simple_counter == 1) {
         // roll
@@ -1750,7 +1750,7 @@ void update_super_simple_bearing()
         // get distance to home
         if(home_distance > SUPER_SIMPLE_RADIUS) {        // 10m from home
             // we reset the angular offset to be a vector from home to the quad
-            initial_simple_bearing = wrap_360(home_bearing+18000);
+            initial_simple_bearing = wrap_360_cd(home_bearing+18000);
         }
     }
 }

ArduCopter/Attitude.pde

@@ -4,7 +4,7 @@ static void
 get_stabilize_roll(int32_t target_angle)
 {
     // angle error
-    target_angle            = wrap_180(target_angle - ahrs.roll_sensor);
+    target_angle            = wrap_180_cd(target_angle - ahrs.roll_sensor);
 
     // limit the error we're feeding to the PID
     target_angle            = constrain(target_angle, -4500, 4500);
@@ -28,7 +28,7 @@ static void
 get_stabilize_pitch(int32_t target_angle)
 {
     // angle error
-    target_angle            = wrap_180(target_angle - ahrs.pitch_sensor);
+    target_angle            = wrap_180_cd(target_angle - ahrs.pitch_sensor);
 
     // limit the error we're feeding to the PID
     target_angle            = constrain(target_angle, -4500, 4500);
@@ -56,7 +56,7 @@ get_stabilize_yaw(int32_t target_angle)
     int32_t output = 0;
 
     // angle error
-    angle_error             = wrap_180(target_angle - ahrs.yaw_sensor);
+    angle_error             = wrap_180_cd(target_angle - ahrs.yaw_sensor);
 
     // limit the error we're feeding to the PID
 
@@ -126,15 +126,15 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
 
     // convert the input to the desired roll rate
     roll_axis += target_rate * G_Dt;
-    roll_axis = wrap_180(roll_axis);
+    roll_axis = wrap_180_cd(roll_axis);
 
     // ensure that we don't reach gimbal lock
     if (labs(roll_axis > 4500) && g.acro_trainer_enabled) {
         roll_axis	= constrain(roll_axis, -4500, 4500);
-        angle_error = wrap_180(roll_axis - ahrs.roll_sensor);
+        angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
     } else {
         // angle error with maximum of +- max_angle_overshoot
-        angle_error = wrap_180(roll_axis - ahrs.roll_sensor);
+        angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
         angle_error	= constrain(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
     }
 
@@ -143,7 +143,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
     }
 
     // update roll_axis to be within max_angle_overshoot of our current heading
-    roll_axis = wrap_180(angle_error + ahrs.roll_sensor);
+    roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor);
 
     // set earth frame targets for rate controller
 
@@ -162,15 +162,15 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
 
     // convert the input to the desired pitch rate
     pitch_axis += target_rate * G_Dt;
-    pitch_axis = wrap_180(pitch_axis);
+    pitch_axis = wrap_180_cd(pitch_axis);
 
     // ensure that we don't reach gimbal lock
     if (labs(pitch_axis) > 4500) {
         pitch_axis	= constrain(pitch_axis, -4500, 4500);
-        angle_error = wrap_180(pitch_axis - ahrs.pitch_sensor);
+        angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
     } else {
         // angle error with maximum of +- max_angle_overshoot
-        angle_error = wrap_180(pitch_axis - ahrs.pitch_sensor);
+        angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
         angle_error	= constrain(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
     }
 
@@ -179,7 +179,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
     }
 
     // update pitch_axis to be within max_angle_overshoot of our current heading
-    pitch_axis = wrap_180(angle_error + ahrs.pitch_sensor);
+    pitch_axis = wrap_180_cd(angle_error + ahrs.pitch_sensor);
 
     // set earth frame targets for rate controller
 	set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME);
@@ -197,10 +197,10 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
 
     // convert the input to the desired yaw rate
     nav_yaw += target_rate * G_Dt;
-    nav_yaw = wrap_360(nav_yaw);
+    nav_yaw = wrap_360_cd(nav_yaw);
 
     // calculate difference between desired heading and current heading
-    angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor);
+    angle_error = wrap_180_cd(nav_yaw - ahrs.yaw_sensor);
 
     // limit the maximum overshoot
     angle_error	= constrain(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
@@ -210,7 +210,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
     }
 
     // update nav_yaw to be within max_angle_overshoot of our current heading
-    nav_yaw = wrap_360(angle_error + ahrs.yaw_sensor);
+    nav_yaw = wrap_360_cd(angle_error + ahrs.yaw_sensor);
 
     // set earth frame targets for rate controller
 	set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
@@ -687,10 +687,10 @@ static void get_look_ahead_yaw(int16_t pilot_yaw)
     // Commanded Yaw to automatically look ahead.
     if (g_gps->fix && g_gps->ground_course > YAW_LOOK_AHEAD_MIN_SPEED) {
         nav_yaw = get_yaw_slew(nav_yaw, g_gps->ground_course, AUTO_YAW_SLEW_RATE);
-        get_stabilize_yaw(wrap_360(nav_yaw + pilot_yaw));   // Allow pilot to "skid" around corners up to 45 degrees
+        get_stabilize_yaw(wrap_360_cd(nav_yaw + pilot_yaw));   // Allow pilot to "skid" around corners up to 45 degrees
     }else{
         nav_yaw += pilot_yaw * g.acro_p * G_Dt;
-        nav_yaw = wrap_360(nav_yaw);
+        nav_yaw = wrap_360_cd(nav_yaw);
         get_stabilize_yaw(nav_yaw);
     }
 }

ArduCopter/GCS_Mavlink.pde

@@ -1881,7 +1881,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         mavlink_msg_hil_state_decode(msg, &packet);
 
         float vel = pythagorous2(packet.vx, packet.vy);
-        float cog = wrap_360(ToDeg(atan2f(packet.vx, packet.vy)) * 100);
+        float cog = wrap_360_cd(ToDeg(atan2f(packet.vx, packet.vy)) * 100);
 
         // set gps hil sensor
         g_gps->setHIL(packet.time_usec/1000,

ArduCopter/commands_logic.pde

@@ -630,10 +630,10 @@ static void do_yaw()
     // get final angle, 1 = Relative, 0 = Absolute
     if( command_cond_queue.lng == 0 ) {
         // absolute angle
-        yaw_look_at_heading = wrap_360(command_cond_queue.alt * 100);
+        yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100);
     }else{
         // relative angle
-        yaw_look_at_heading = wrap_360(nav_yaw + command_cond_queue.alt * 100);
+        yaw_look_at_heading = wrap_360_cd(nav_yaw + command_cond_queue.alt * 100);
     }
 
     // get turn speed
@@ -641,7 +641,7 @@ static void do_yaw()
         // default to regular auto slew rate
         yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
     }else{
-        int32_t turn_rate = (wrap_180(yaw_look_at_heading - nav_yaw) / 100) / command_cond_queue.lat;
+        int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - nav_yaw) / 100) / command_cond_queue.lat;
         yaw_look_at_heading_slew = constrain(turn_rate, 1, 360);    // deg / sec
     }
 
@@ -688,7 +688,7 @@ static bool verify_within_distance()
 // verify_yaw - return true if we have reached the desired heading
 static bool verify_yaw()
 {
-    if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200 ) {
+    if( labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200 ) {
         return true;
     }else{
         return false;
@@ -706,7 +706,7 @@ static bool verify_nav_roi()
     // check if mount type requires us to rotate the quad
     if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) {
         // ensure yaw has gotten to within 2 degrees of the target
-        if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
+        if( labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
             return true;
         }else{
             return false;
@@ -718,7 +718,7 @@ static bool verify_nav_roi()
 #else
     // if we have no camera mount simply check we've reached the desired yaw
     // ensure yaw has gotten to within 2 degrees of the target
-    if( labs(wrap_180(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
+    if( labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_WP_bearing)) <= 200 ) {
         return true;
     }else{
         return false;

ArduCopter/navigation.pde

@@ -209,7 +209,7 @@ static bool check_missed_wp()
 {
     int32_t temp;
     temp = wp_bearing - original_wp_bearing;
-    temp = wrap_180(temp);
+    temp = wrap_180_cd(temp);
     return (labs(temp) > 9000);         // we passed the waypoint by 90 degrees
 }
 
@@ -279,25 +279,11 @@ static void reset_nav_params(void)
     auto_pitch 						= 0;
 }
 
-static int32_t wrap_360(int32_t error)
-{
-    if (error > 36000) error -= 36000;
-    if (error < 0) error += 36000;
-    return error;
-}
-
-static int32_t wrap_180(int32_t error)
-{
-    if (error > 18000) error -= 36000;
-    if (error < -18000) error += 36000;
-    return error;
-}
-
 // get_yaw_slew - reduces rate of change of yaw to a maximum
 // assumes it is called at 100hz so centi-degrees and update rate cancel each other out
 static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t deg_per_sec)
 {
-    return wrap_360(current_yaw + constrain(wrap_180(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
+    return wrap_360_cd(current_yaw + constrain(wrap_180_cd(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
 }
 
 // valid_waypoint - checks if a waypoint has been initialised or not
@@ -621,4 +607,4 @@ circle_get_pos(float dt)
 
     // re-use loiter position controller
     get_loiter_pos_lat_lon(circle_target.x, circle_target.y, dt);
-}
+}

ArduCopter/test.pde

@@ -847,7 +847,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             if (compass.read()) {
                 float heading = compass.calculate_heading(ahrs.get_dcm_matrix());
                 cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
-                                (wrap_360(ToDeg(heading) * 100)) /100,
+                                (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                 compass.mag_x,
                                 compass.mag_y,
                                 compass.mag_z);