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

Copter: tilt correct rangefinder

This commit is contained in:
Randy Mackay 2016-04-28 12:32:46 +09:00
parent ba38b0234f
commit c7f80324fa
2 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/config.h
View File

@ -150,7 +150,7 @@
#endif
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define RANGEFINDER_TILT_CORRECTION DISABLED
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif

6
ArduCopter/sensors.cpp
View File

@ -49,11 +49,9 @@ void Copter::read_rangefinder(void)
int16_t temp_alt = rangefinder.distance_cm();
#if RANGEFINDER_TILT_CORRECTION == 1
#if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder
float temp = ahrs.cos_pitch() * ahrs.cos_roll();
temp = MAX(temp, 0.707f);
temp_alt = (float)temp_alt * temp;
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#endif
rangefinder_alt = temp_alt;