From 6fd4a68c5cccbb0c82cf26b17cf5ea5837f193e1 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Thu, 16 Aug 2012 17:50:15 -0700 Subject: [PATCH] uncrustify ArduPlane/navigation.pde --- ArduPlane/navigation.pde | 178 +++++++++++++++++++-------------------- 1 file changed, 89 insertions(+), 89 deletions(-) diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index d4daa289d2..95b3ac3506 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -5,48 +5,48 @@ //**************************************************************** static void navigate() { - // do not navigate with corrupt data - // --------------------------------- - if (!have_position) { - return; - } + // do not navigate with corrupt data + // --------------------------------- + if (!have_position) { + return; + } - if(next_WP.lat == 0){ - return; - } + if(next_WP.lat == 0) { + return; + } - // waypoint distance from plane - // ---------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); + // waypoint distance from plane + // ---------------------------- + wp_distance = get_distance(¤t_loc, &next_WP); - if (wp_distance < 0) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - return; - } + if (wp_distance < 0) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0")); + //Serial.println(wp_distance,DEC); + return; + } - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP); + // target_bearing is where we should be heading + // -------------------------------------------- + target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP); - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing_cd = target_bearing_cd; + // nav_bearing will includes xtrac correction + // ------------------------------------------ + nav_bearing_cd = target_bearing_cd; - // check if we have missed the WP - loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100; + // check if we have missed the WP + loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100; - // reset the old value - old_target_bearing_cd = target_bearing_cd; + // reset the old value + old_target_bearing_cd = target_bearing_cd; - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); + // wrap values + if (loiter_delta > 180) loiter_delta -= 360; + if (loiter_delta < -180) loiter_delta += 360; + loiter_sum += abs(loiter_delta); - // control mode specific updates to nav_bearing - // -------------------------------------------- - update_navigation(); + // control mode specific updates to nav_bearing + // -------------------------------------------- + update_navigation(); } @@ -54,9 +54,9 @@ static void navigate() // Disabled for now void calc_distance_error() { - distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01)); - distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); - wp_distance = max(distance_estimate,10); + distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01)); + distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); + wp_distance = max(distance_estimate,10); } #endif @@ -70,9 +70,9 @@ static void calc_airspeed_errors() // FBW_B airspeed target if (control_mode == FLY_BY_WIRE_B) { target_airspeed_cm = ((int)(g.flybywire_airspeed_max - - g.flybywire_airspeed_min) * - g.channel_throttle.servo_out) + - ((int)g.flybywire_airspeed_min * 100); + g.flybywire_airspeed_min) * + g.channel_throttle.servo_out) + + ((int)g.flybywire_airspeed_min * 100); } // Set target to current airspeed + ground speed undershoot, @@ -106,85 +106,85 @@ static void calc_gndspeed_undershoot() static void calc_bearing_error() { bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor; - bearing_error_cd = wrap_180_cd(bearing_error_cd); + bearing_error_cd = wrap_180_cd(bearing_error_cd); } static void calc_altitude_error() { - if(control_mode == AUTO && offset_altitude_cm != 0) { - // limit climb rates - target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30)); + if(control_mode == AUTO && offset_altitude_cm != 0) { + // limit climb rates + target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30)); - // stay within a certain range - if(prev_WP.alt > next_WP.alt){ - target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt); - }else{ - target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt); - } - } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { - target_altitude_cm = next_WP.alt; - } + // stay within a certain range + if(prev_WP.alt > next_WP.alt) { + target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt); + }else{ + target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt); + } + } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { + target_altitude_cm = next_WP.alt; + } - altitude_error_cm = target_altitude_cm - current_loc.alt; + altitude_error_cm = target_altitude_cm - current_loc.alt; } static int32_t wrap_360_cd(int32_t error) { - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; + if (error > 36000) error -= 36000; + if (error < 0) error += 36000; + return error; } static int32_t wrap_180_cd(int32_t error) { - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; + if (error > 18000) error -= 36000; + if (error < -18000) error += 36000; + return error; } static void update_loiter() { - float power; + float power; - if(wp_distance <= g.loiter_radius){ - power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); - nav_bearing_cd += 9000.0 * (2.0 + power); - } else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing_cd -= power * 9000; - } else{ - update_crosstrack(); - loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit + if(wp_distance <= g.loiter_radius) { + power = float(wp_distance) / float(g.loiter_radius); + power = constrain(power, 0.5, 1); + nav_bearing_cd += 9000.0 * (2.0 + power); + } else if(wp_distance < (g.loiter_radius + LOITER_RANGE)) { + power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); + nav_bearing_cd -= power * 9000; + } else{ + update_crosstrack(); + loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit - } + } /* - if (wp_distance < g.loiter_radius){ - nav_bearing += 9000; - }else{ - nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); - } - - update_crosstrack(); -*/ - nav_bearing_cd = wrap_360_cd(nav_bearing_cd); + * if (wp_distance < g.loiter_radius){ + * nav_bearing += 9000; + * }else{ + * nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); + * } + * + * update_crosstrack(); + */ + nav_bearing_cd = wrap_360_cd(nav_bearing_cd); } static void update_crosstrack(void) { - // Crosstrack Error - // ---------------- + // Crosstrack Error + // ---------------- // If we are too far off or too close we don't do track following - if (abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { - crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; // Meters we are off track line - nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing_cd = wrap_360_cd(nav_bearing_cd); - } + if (abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { + crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; // Meters we are off track line + nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); + nav_bearing_cd = wrap_360_cd(nav_bearing_cd); + } } static void reset_crosstrack() { - crosstrack_bearing_cd = get_bearing_cd(&prev_WP, &next_WP); // Used for track following + crosstrack_bearing_cd = get_bearing_cd(&prev_WP, &next_WP); // Used for track following }