uncrustify ArduPlane/navigation.pde
This commit is contained in:
uncrustify
Pat Hickey
parent
8e3fe50338
commit
6fd4a68c5c
@ -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
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user