uncrustify ArduPlane/navigation.pde

ArduPlane/navigation.pde

@@ -5,48 +5,48 @@
 //****************************************************************
 static void navigate()
 {
-	// do not navigate with corrupt data
-	// ---------------------------------
-	if (!have_position) {
-		return;
-	}
-
-	if(next_WP.lat == 0){
-		return;
-	}
-
-	// waypoint distance from plane
-	// ----------------------------
-	wp_distance = get_distance(&current_loc, &next_WP);
-
-	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(&current_loc, &next_WP);
-
-	// 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;
-
-	// 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);
-
-	// control mode specific updates to nav_bearing
-	// --------------------------------------------
-	update_navigation();
+    // do not navigate with corrupt data
+    // ---------------------------------
+    if (!have_position) {
+        return;
+    }
+
+    if(next_WP.lat == 0) {
+        return;
+    }
+
+    // waypoint distance from plane
+    // ----------------------------
+    wp_distance = get_distance(&current_loc, &next_WP);
+
+    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(&current_loc, &next_WP);
+
+    // 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;
+
+    // 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);
+
+    // 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));
-
-		// 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;
+    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;
+    }
+
+    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;
-
-	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
-
-	}
+    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){
-		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
 }