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

AP_Landing: move location_update to Location and rename to offset_bearing

Browse Source
This commit is contained in:
Pierre Kancir 2019-04-05 08:11:26 +02:00 committed by Peter Barker
parent a30404fb11
commit 12a357ffd7
3 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Landing/AP_Landing.h
View File

@ -21,6 +21,7 @@
#include <AP_SpdHgtControl/AP_SpdHgtControl.h> #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_Navigation/AP_Navigation.h> #include <AP_Navigation/AP_Navigation.h>
#include "AP_Landing_Deepstall.h" #include "AP_Landing_Deepstall.h"
#include <AP_Common/Location.h>
/// @class AP_Landing /// @class AP_Landing
/// @brief Class managing ArduPlane landing methods /// @brief Class managing ArduPlane landing methods

11
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -21,6 +21,7 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <AP_Common/Location.h>
// table of user settable parameters for deepstall // table of user settable parameters for deepstall
const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = { const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = {
@ -278,7 +279,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, false); const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, false);
memcpy(&entry_point, &landing_point, sizeof(Location)); memcpy(&entry_point, &landing_point, sizeof(Location));
location_update(entry_point, target_heading_deg + 180.0, travel_distance); entry_point.offset_bearing(target_heading_deg + 180.0, travel_distance);
if (!location_passed_point(current_loc, arc_exit, entry_point)) { if (!location_passed_point(current_loc, arc_exit, entry_point)) {
if (location_passed_point(current_loc, arc_exit, extended_approach)) { if (location_passed_point(current_loc, arc_exit, extended_approach)) {
@ -488,7 +489,7 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
memcpy(&arc_exit, &landing_point, sizeof(Location)); memcpy(&arc_exit, &landing_point, sizeof(Location));
//extend the approach point to 1km away so that there is always a navigational target //extend the approach point to 1km away so that there is always a navigational target
location_update(extended_approach, target_heading_deg, 1000.0); extended_approach.offset_bearing(target_heading_deg, 1000.0);
float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ? landing_point.alt * 0.01f : approach_alt_offset, float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ? landing_point.alt * 0.01f : approach_alt_offset,
false); false);
@ -498,13 +499,13 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
// decent chance to be misaligned on final approach // decent chance to be misaligned on final approach
approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f); approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f);
location_update(arc_exit, target_heading_deg + 180, approach_extension_m); arc_exit.offset_bearing(target_heading_deg + 180, approach_extension_m);
memcpy(&arc, &arc_exit, sizeof(Location)); memcpy(&arc, &arc_exit, sizeof(Location));
memcpy(&arc_entry, &arc_exit, sizeof(Location)); memcpy(&arc_entry, &arc_exit, sizeof(Location));
float arc_heading_deg = target_heading_deg + (landing_point.loiter_ccw ? -90.0f : 90.0f); float arc_heading_deg = target_heading_deg + (landing_point.loiter_ccw ? -90.0f : 90.0f);
location_update(arc, arc_heading_deg, loiter_radius_m_abs); arc.offset_bearing(arc_heading_deg, loiter_radius_m_abs);
location_update(arc_entry, arc_heading_deg, loiter_radius_m_abs * 2); arc_entry.offset_bearing(arc_heading_deg, loiter_radius_m_abs * 2);
#ifdef DEBUG_PRINTS #ifdef DEBUG_PRINTS
// TODO: Send this information via a MAVLink packet // TODO: Send this information via a MAVLink packet

8
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -138,9 +138,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
struct Location land_WP_loc = next_WP_loc; struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc); int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
location_update(land_WP_loc, land_WP_loc.offset_bearing(land_bearing_cd * 0.01f, prev_WP_loc.get_distance(current_loc) + 200);
land_bearing_cd*0.01f,
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
@ -294,11 +292,11 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
// now calculate our aim point, which is before the landing // now calculate our aim point, which is before the landing
// point and above it // point and above it
Location loc = next_WP_loc; Location loc = next_WP_loc;
location_update(loc, land_bearing_cd*0.01f, -flare_distance); loc.offset_bearing(land_bearing_cd * 0.01f, -flare_distance);
loc.alt += aim_height*100; loc.alt += aim_height*100;
// calculate point along that slope 500m ahead // calculate point along that slope 500m ahead
location_update(loc, land_bearing_cd*0.01f, land_projection); loc.offset_bearing(land_bearing_cd * 0.01f, land_projection);
loc.alt -= slope * land_projection * 100; loc.alt -= slope * land_projection * 100;
// setup the offset_cm for set_target_altitude_proportion() // setup the offset_cm for set_target_altitude_proportion()