Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: replace location_offset() and get_distance() function calls with Location object member function calls

Browse Source 
This allows removing duplicated code
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2019-02-25 01:16:22 +01:00 committed by Peter Barker
parent d7edd396bd
commit 003a346ee2
3 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Landing/AP_Landing.h
View File

@ -117,7 +117,7 @@ private:
// same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope
float initial_slope; float initial_slope;
// calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc) // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / prev_WP_loc.get_distance(next_WP_loc)
float slope; float slope;
AP_Mission &mission; AP_Mission &mission;

4
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -187,7 +187,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
{ {
switch (stage) { switch (stage) {
case DEEPSTALL_STAGE_FLY_TO_LANDING: case DEEPSTALL_STAGE_FLY_TO_LANDING:
if (get_distance(current_loc, landing_point) > abs(2 * landing.aparm.loiter_radius)) { if (current_loc.get_distance(landing_point) > abs(2 * landing.aparm.loiter_radius)) {
landing.nav_controller->update_waypoint(current_loc, landing_point); landing.nav_controller->update_waypoint(current_loc, landing_point);
return false; return false;
} }
@ -239,7 +239,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
memcpy(&breakout_location, &current_loc, sizeof(Location)); memcpy(&breakout_location, &current_loc, sizeof(Location));
FALLTHROUGH; FALLTHROUGH;
case DEEPSTALL_STAGE_FLY_TO_ARC: case DEEPSTALL_STAGE_FLY_TO_ARC:
if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) { if (current_loc.get_distance(arc_entry) > 2 * landing.aparm.loiter_radius) {
landing.nav_controller->update_waypoint(breakout_location, arc_entry); landing.nav_controller->update_waypoint(breakout_location, arc_entry);
return false; return false;
} }

10
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -100,7 +100,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
(double)height, (double)sink_rate, (double)height, (double)sink_rate,
(double)AP::gps().ground_speed(), (double)AP::gps().ground_speed(),
(double)get_distance(current_loc, next_WP_loc)); (double)current_loc.get_distance(next_WP_loc));
} }
type_slope_stage = SLOPE_STAGE_FINAL; type_slope_stage = SLOPE_STAGE_FINAL;
@ -139,14 +139,14 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc, location_update(land_WP_loc,
land_bearing_cd*0.01f, land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200); prev_WP_loc.get_distance(current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
// once landed and stationary, post some statistics // once landed and stationary, post some statistics
// this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
if (type_slope_flags.post_stats && !is_armed) { if (type_slope_flags.post_stats && !is_armed) {
type_slope_flags.post_stats = false; type_slope_flags.post_stats = false;
gcs().send_text(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); gcs().send_text(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc));
} }
// check if we should auto-disarm after a confirmed landing // check if we should auto-disarm after a confirmed landing
@ -179,7 +179,7 @@ void AP_Landing::type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle
rangefinder_state.last_stable_correction = rangefinder_state.correction; rangefinder_state.last_stable_correction = rangefinder_state.correction;
float corrected_alt_m = (adjusted_altitude_cm_fn() - next_WP_loc.alt)*0.01f - rangefinder_state.correction; float corrected_alt_m = (adjusted_altitude_cm_fn() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
float total_distance_m = get_distance(prev_WP_loc, next_WP_loc); float total_distance_m = prev_WP_loc.get_distance(next_WP_loc);
float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / wp_distance; float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / wp_distance;
prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt; prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
@ -229,7 +229,7 @@ bool AP_Landing::type_slope_request_go_around(void)
*/ */
void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm) void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
{ {
float total_distance = get_distance(prev_WP_loc, next_WP_loc); float total_distance = prev_WP_loc.get_distance(next_WP_loc);
// If someone mistakenly puts all 0's in their LAND command then total_distance // If someone mistakenly puts all 0's in their LAND command then total_distance
// will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that.