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

Copter: use rangefinder class's valid_range_count

Browse Source 
Also read_rangefinder directly updates rangefinder_alt variable instead of returning distance
No functional change
This commit is contained in:
Randy Mackay 2016-04-27 21:20:03 +09:00
parent 949d5f7109
commit ba38b0234f
3 changed files with 9 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.cpp
View File

@ -614,7 +614,7 @@ void Copter::update_altitude()
read_barometer(); read_barometer();
// read in rangefinder altitude // read in rangefinder altitude
rangefinder_alt = read_rangefinder(); read_rangefinder();
// write altitude info to dataflash logs // write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN)) { if (should_log(MASK_LOG_CTUN)) {

2
ArduCopter/Copter.h
View File

@ -946,7 +946,7 @@ private:
void init_barometer(bool full_calibration); void init_barometer(bool full_calibration);
void read_barometer(void); void read_barometer(void);
void init_rangefinder(void); void init_rangefinder(void);
int16_t read_rangefinder(void); void read_rangefinder(void);
bool rangefinder_alt_ok(); bool rangefinder_alt_ok();
void init_compass(); void init_compass();
void init_optflow(); void init_optflow();

20
ArduCopter/sensors.cpp
View File

@ -34,7 +34,7 @@ void Copter::init_rangefinder(void)
} }
// return rangefinder altitude in centimeters // return rangefinder altitude in centimeters
int16_t Copter::read_rangefinder(void) void Copter::read_rangefinder(void)
{ {
#if RANGEFINDER_ENABLED == ENABLED #if RANGEFINDER_ENABLED == ENABLED
rangefinder.update(); rangefinder.update();
@ -42,20 +42,13 @@ int16_t Copter::read_rangefinder(void)
// exit immediately if rangefinder is disabled // exit immediately if rangefinder is disabled
if (rangefinder.status() != RangeFinder::RangeFinder_Good) { if (rangefinder.status() != RangeFinder::RangeFinder_Good) {
rangefinder_alt_health = 0; rangefinder_alt_health = 0;
return 0; return;
} }
rangefinder_alt_health = rangefinder.range_valid_count();
int16_t temp_alt = rangefinder.distance_cm(); int16_t temp_alt = rangefinder.distance_cm();
if (temp_alt >= rangefinder.min_distance_cm() &&
temp_alt <= rangefinder.max_distance_cm() * RANGEFINDER_RELIABLE_DISTANCE_PCT) {
if (rangefinder_alt_health < RANGEFINDER_HEALTH_MAX) {
rangefinder_alt_health++;
}
}else{
rangefinder_alt_health = 0;
}
#if RANGEFINDER_TILT_CORRECTION == 1 #if RANGEFINDER_TILT_CORRECTION == 1
// correct alt for angle of the rangefinder // correct alt for angle of the rangefinder
float temp = ahrs.cos_pitch() * ahrs.cos_roll(); float temp = ahrs.cos_pitch() * ahrs.cos_roll();
@ -63,9 +56,10 @@ int16_t Copter::read_rangefinder(void)
temp_alt = (float)temp_alt * temp; temp_alt = (float)temp_alt * temp;
#endif #endif
return temp_alt; rangefinder_alt = temp_alt;
#else #else
return 0; rangefinder_alt_health = 0;
rangefinder_alt = 0;
#endif #endif
} }