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

Copter: Use filtered and corrected range finder in surface tracking

This commit is contained in:
Leonard Hall 2023-01-08 19:27:06 +10:30 committed by Andrew Tridgell
parent d946fc59da
commit d56f476789
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/surface_tracking.cpp
View File

@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset()
// e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m. curr_surface_alt_above_origin_cm is 7m (or 700cm)
RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm;
const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get();
// update position controller target offset to the surface's alt above the EKF origin
copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);