Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Landing: Deepstall print travel distances 

The distances are used for doing log analysis on the prediction numbers.
This commit is contained in:
Michael du Breuil 2017-04-13 12:24:48 -07:00 committed by Tom Pittenger
parent 379a4fbfab
commit 26ff1acf55
2 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -241,7 +241,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
float relative_alt_D;
landing.ahrs.get_relative_position_D_home(relative_alt_D);
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D);
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false);
memcpy(&entry_point, &landing_point, sizeof(Location));
location_update(entry_point, target_heading_deg + 180.0, travel_distance);
@ -253,6 +253,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
}
return false;
}
predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true);
stage = DEEPSTALL_STAGE_LAND;
stall_entry_time = AP_HAL::millis();
@ -401,7 +402,7 @@ void AP_Landing_Deepstall::build_approach_path(void)
//extend the approach point to 1km away so that there is always a navigational target
location_update(extended_approach, target_heading_deg, 1000.0);
float expected_travel_distance = predict_travel_distance(wind, landing_point.alt / 100);
float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false);
float approach_extension_m = expected_travel_distance + approach_extension;
// an approach extensions must be at least half the loiter radius, or the aircraft has a
// decent chance to be misaligned on final approach
@ -432,7 +433,7 @@ void AP_Landing_Deepstall::build_approach_path(void)
}
float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height)
float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height, const bool print)
{
bool reverse = false;
@ -464,11 +465,19 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f
float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length;
predicted_travel_distance = estimated_forward * height / down_speed + stall_distance;
if (is_positive(down_speed)) {
predicted_travel_distance = (estimated_forward * height / down_speed) + stall_distance;
} else {
// if we don't have a sane downward speed in a deepstall (IE not zero, and not
// an ascent) then just provide the stall_distance as a reasonable approximation
predicted_travel_distance = stall_distance;
}
#ifdef DEBUG_PRINTS
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance, predicted_travel_distance);
#endif // DEBUG_PRINTS
if(print) {
// allow printing the travel distances on the final entry as its used for tuning
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)",
(double)stall_distance, (double)predicted_travel_distance);
}
return predicted_travel_distance;
}

2
libraries/AP_Landing/AP_Landing_Deepstall.h
View File

@ -106,7 +106,7 @@ private:
//private helpers
void build_approach_path();
float predict_travel_distance(const Vector3f wind, const float height);
float predict_travel_distance(const Vector3f wind, const float height, const bool print);
bool verify_breakout(const Location &current_loc, const Location &target_loc, const float height_error) const;
float update_steering(void);