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

Copter: tilt correct rangefinder

Browse Source
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 #endif
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF #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 #endif

6
ArduCopter/sensors.cpp
View File

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