aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uncrustify ArduPlane/navigation.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 17:50:15 -07:00 committed by Pat Hickey
parent 8e3fe50338
commit 6fd4a68c5c
1 changed files with 89 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
ArduPlane/navigation.pde
View File

@ -5,48 +5,48 @@
//**************************************************************** //****************************************************************
static void navigate() static void navigate()
{ {
// do not navigate with corrupt data // do not navigate with corrupt data
// --------------------------------- // ---------------------------------
if (!have_position) { if (!have_position) {
return; return;
} }
if(next_WP.lat == 0){ if(next_WP.lat == 0) {
return; return;
} }
// waypoint distance from plane // waypoint distance from plane
// ---------------------------- // ----------------------------
wp_distance = get_distance(&current_loc, &next_WP); wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0) { if (wp_distance < 0) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0")); gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0"));
//Serial.println(wp_distance,DEC); //Serial.println(wp_distance,DEC);
return; return;
} }
// target_bearing is where we should be heading // target_bearing is where we should be heading
// -------------------------------------------- // --------------------------------------------
target_bearing_cd = get_bearing_cd(&current_loc, &next_WP); target_bearing_cd = get_bearing_cd(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction // nav_bearing will includes xtrac correction
// ------------------------------------------ // ------------------------------------------
nav_bearing_cd = target_bearing_cd; nav_bearing_cd = target_bearing_cd;
// check if we have missed the WP // check if we have missed the WP
loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100; loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100;
// reset the old value // reset the old value
old_target_bearing_cd = target_bearing_cd; old_target_bearing_cd = target_bearing_cd;
// wrap values // wrap values
if (loiter_delta > 180) loiter_delta -= 360; if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360; if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta); loiter_sum += abs(loiter_delta);
// control mode specific updates to nav_bearing // control mode specific updates to nav_bearing
// -------------------------------------------- // --------------------------------------------
update_navigation(); update_navigation();
} }
@ -54,9 +54,9 @@ static void navigate()
// Disabled for now // Disabled for now
void calc_distance_error() void calc_distance_error()
{ {
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01)); 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); distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
wp_distance = max(distance_estimate,10); wp_distance = max(distance_estimate,10);
} }
#endif #endif
@ -70,9 +70,9 @@ static void calc_airspeed_errors()
// FBW_B airspeed target // FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B) { if (control_mode == FLY_BY_WIRE_B) {
target_airspeed_cm = ((int)(g.flybywire_airspeed_max - target_airspeed_cm = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) * g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) + g.channel_throttle.servo_out) +
((int)g.flybywire_airspeed_min * 100); ((int)g.flybywire_airspeed_min * 100);
} }
// Set target to current airspeed + ground speed undershoot, // Set target to current airspeed + ground speed undershoot,
@ -106,85 +106,85 @@ static void calc_gndspeed_undershoot()
static void calc_bearing_error() static void calc_bearing_error()
{ {
bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor; 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() static void calc_altitude_error()
{ {
if(control_mode == AUTO && offset_altitude_cm != 0) { if(control_mode == AUTO && offset_altitude_cm != 0) {
// limit climb rates // limit climb rates
target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30)); target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30));
// stay within a certain range // stay within a certain range
if(prev_WP.alt > next_WP.alt){ if(prev_WP.alt > next_WP.alt) {
target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt); target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt);
}else{ }else{
target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt); target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt);
} }
} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude_cm = next_WP.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) static int32_t wrap_360_cd(int32_t error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;
if (error < 0) error += 36000; if (error < 0) error += 36000;
return error; return error;
} }
static int32_t wrap_180_cd(int32_t error) static int32_t wrap_180_cd(int32_t error)
{ {
if (error > 18000) error -= 36000; if (error > 18000) error -= 36000;
if (error < -18000) error += 36000; if (error < -18000) error += 36000;
return error; return error;
} }
static void update_loiter() static void update_loiter()
{ {
float power; float power;
if(wp_distance <= g.loiter_radius){ if(wp_distance <= g.loiter_radius) {
power = float(wp_distance) / float(g.loiter_radius); power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1); power = constrain(power, 0.5, 1);
nav_bearing_cd += 9000.0 * (2.0 + power); nav_bearing_cd += 9000.0 * (2.0 + power);
} else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ } else if(wp_distance < (g.loiter_radius + LOITER_RANGE)) {
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing_cd -= power * 9000; nav_bearing_cd -= power * 9000;
} else{ } else{
update_crosstrack(); update_crosstrack();
loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
} }
/* /*
if (wp_distance < g.loiter_radius){ * if (wp_distance < g.loiter_radius){
nav_bearing += 9000; * nav_bearing += 9000;
}else{ * }else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); * nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
} * }
*
update_crosstrack(); * update_crosstrack();
*/ */
nav_bearing_cd = wrap_360_cd(nav_bearing_cd); nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
} }
static void update_crosstrack(void) 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 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) { 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 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 += 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); nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
} }
} }
static void reset_crosstrack() 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
} }