Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: update for new double precision position APIs

Browse Source
This commit is contained in:
Andrew Tridgell 2021-06-18 11:20:26 +10:00
parent bada2670a6
commit 50e6d67a66
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/quadplane.cpp
View File

@ -2547,7 +2547,7 @@ void QuadPlane::update_land_positioning(void)
poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms; poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms;
poscontrol.target_vel_cms.rotate_xy(ahrs_view->yaw); poscontrol.target_vel_cms.rotate_xy(ahrs_view->yaw);
poscontrol.target_cm += poscontrol.target_vel_cms * dt; poscontrol.target_cm += (poscontrol.target_vel_cms * dt).topostype();
poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in)); poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in));
if (poscontrol.pilot_correction_active) { if (poscontrol.pilot_correction_active) {
@ -3091,7 +3091,7 @@ void QuadPlane::setup_target_position(void)
if (!loc.same_latlon_as(last_auto_target) || if (!loc.same_latlon_as(last_auto_target) ||
plane.next_WP_loc.alt != last_auto_target.alt || plane.next_WP_loc.alt != last_auto_target.alt ||
now - last_loiter_ms > 500) { now - last_loiter_ms > 500) {
wp_nav->set_wp_destination(poscontrol.target_cm); wp_nav->set_wp_destination(poscontrol.target_cm.tofloat());
last_auto_target = loc; last_auto_target = loc;
} }
last_loiter_ms = now; last_loiter_ms = now;
@ -3500,7 +3500,7 @@ bool QuadPlane::verify_vtol_land(void)
if (poscontrol.pilot_correction_done) { if (poscontrol.pilot_correction_done) {
reached_position = !poscontrol.pilot_correction_active; reached_position = !poscontrol.pilot_correction_active;
} else { } else {
const float dist = (inertial_nav.get_position() - poscontrol.target_cm).xy().length() * 0.01; const float dist = (inertial_nav.get_position().topostype() - poscontrol.target_cm).xy().length() * 0.01;
reached_position = dist < descend_dist_threshold; reached_position = dist < descend_dist_threshold;
} }
if (reached_position && if (reached_position &&

2
ArduPlane/quadplane.h
View File

@ -478,7 +478,7 @@ private:
uint32_t time_since_state_start_ms() const { uint32_t time_since_state_start_ms() const {
return AP_HAL::millis() - last_state_change_ms; return AP_HAL::millis() - last_state_change_ms;
} }
Vector3f target_cm; Vector3p target_cm;
Vector3f target_vel_cms; Vector3f target_vel_cms;
bool slow_descent:1; bool slow_descent:1;
bool pilot_correction_active; bool pilot_correction_active;