Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

rename/removed global var sonarAngle to tiltAngle , fix spaces/lines

Browse source 
Conflicts:

	src/imu.c
	src/main/drivers/sonar_hcsr04.c
This commit is contained in:
treymarc 2014-06-22 23:19:01 +00:00 • committed by Dominic Clifton
parent 5253064b46
commit ae0f842266
4 changed files with 666 additions and 660 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/drivers/sonar_hcsr04.c
View file

@ -60,13 +60,11 @@ void ECHO_EXTI_IRQHandler(void)
//
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
int32_t distance = (timing_stop - timing_start) / 59;
// this sonar range is up to 4meter , but 3meter is accurate enough (+tilted and roll)
// this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
if (distance > 300)
distance = -1;
*distance_ptr = distance;
}
}

14
src/main/flight/imu.c
View file

@ -370,6 +370,11 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
}
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
{
return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
}
int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
uint32_t newBaroPID = 0;
@ -420,6 +425,8 @@ void calculateEstimatedAltitude(uint32_t currentTime)
static int32_t baroAlt_offset = 0;
float sonarTransition;
int16_t tiltAngle;
dTime = currentTime - previousTime;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
@ -432,8 +439,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
vel = 0;
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
sonarAlt = sonarCalculateAltitude(sonarAlt, &inclination);
tiltAngle = calculateTiltAngle(&inclination);
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
if (sonarAlt > 0 && sonarAlt < 200) {
baroAlt_offset = BaroAlt - sonarAlt;
@ -446,7 +456,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
}
}
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
@ -471,6 +480,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
}
if (sonarAlt > 0 && sonarAlt < 200) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;

10
src/main/sensors/sonar.c
View file

@ -47,15 +47,13 @@ void Sonar_update(void)
hcsr04_get_distance(&sonarAlt);
}
int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle)
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
{
// calculate sonar altitude
int16_t sonarAnglecorrection = max(abs(angle->values.rollDeciDegrees), abs(angle->values.pitchDeciDegrees));
if (sonarAnglecorrection > 250)
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
if (tiltAngle > 250)
return -1;
return sonarAlt * (900.0f - sonarAnglecorrection) / 900.0f;
return sonarAlt * (900.0f - tiltAngle) / 900.0f;
}
#endif

2
src/main/sensors/sonar.h
View file

@ -22,4 +22,4 @@ extern int32_t sonarAlt;
void Sonar_init(void);
void Sonar_update(void);
int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle);
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);