Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Merge branch 'development' into mateksys-f405-i2c

Browse source
This commit is contained in:
Jonathan Hudson 2017-07-22 17:18:34 +01:00
commit c396470f67
7 changed files with 34 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1148,7 +1148,7 @@ static void loadMainState(timeUs_t currentTimeUs)
#ifdef USE_RANGEFINDER
// Store the raw rangefinder surface readout without applying tilt correction
blackboxCurrent->surfaceRaw = rangefinderRead();
blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
#endif
blackboxCurrent->rssi = rssi;

7
src/main/fc/fc_tasks.c
View file

@ -184,7 +184,12 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
rescheduleTask(TASK_SELF, newDeadline);
}
updatePositionEstimator_SurfaceTopic(currentTimeUs);
/*
* Process raw rangefinder readout
*/
rangefinderProcess(calculateCosTiltAngle());
updatePositionEstimator_SurfaceTopic(currentTimeUs, rangefinderGetLatestAltitude());
}
#endif

2
src/main/navigation/navigation.h
View file

@ -248,7 +248,7 @@ void navigationInit(void);
/* Position estimator update functions */
void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs);
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt);
/* Navigation system updates */
void updateWaypointsAndNavigationMode(void);

5
src/main/navigation/navigation_pos_estimator.c
View file

@ -474,11 +474,8 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
* Read surface and update alt/vel topic
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
*/
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs)
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
{
float newSurfaceAlt = rangefinderRead();
newSurfaceAlt = rangefinderCalculateAltitude(newSurfaceAlt, calculateCosTiltAngle());
if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
posEstimator.surface.alt = newSurfaceAlt;
posEstimator.surface.lastUpdateTime = currentTimeUs;

35
src/main/sensors/rangefinder.c
View file

@ -188,7 +188,7 @@ timeDelta_t rangefinderUpdate(void)
/**
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
*/
int32_t rangefinderRead(void)
void rangefinderProcess(float cosTiltAngle)
{
if (rangefinder.dev.read) {
const int32_t distance = rangefinder.dev.read();
@ -213,31 +213,24 @@ int32_t rangefinderRead(void)
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
}
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
return rangefinder.rawAltitude;
}
/**
* Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
* the altitude. Returns the computed altitude in centimeters.
*
* When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
*/
int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle)
{
// calculate sonar altitude only if the ground is in the sonar cone
if (cosTiltAngle < rangefinder.maxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
/**
* Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
* the altitude. Returns the computed altitude in centimeters.
*
* When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
*/
if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
} else {
rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle;
rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
}
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
return rangefinder.calculatedAltitude;
}
/**
* Get the latest altitude that was computed by a call to rangefinderCalculateAltitude(), or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
* Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
* has never been called.
*/
int32_t rangefinderGetLatestAltitude(void)
@ -245,6 +238,10 @@ int32_t rangefinderGetLatestAltitude(void)
return rangefinder.calculatedAltitude;
}
int32_t rangefinderGetLatestRawAltitude(void) {
return rangefinder.rawAltitude;
}
bool rangefinderIsHealthy(void)
{
return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;

6
src/main/sensors/rangefinder.h
View file

@ -48,11 +48,9 @@ const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void);
bool rangefinderInit(void);
int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle);
int32_t rangefinderGetLatestAltitude(void);
int32_t rangefinderGetLatestRawAltitude(void);
timeDelta_t rangefinderUpdate(void);
int32_t rangefinderRead(void);
void rangefinderProcess(float cosTiltAngle);
bool rangefinderIsHealthy(void);

15
src/main/target/MATEKF405/target.c
View file

@ -24,14 +24,13 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM5, IO_TAG(PA3), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM5, TIM_USE_PPM },
{ TIM3, IO_TAG(PC6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR },
{ TIM8, IO_TAG(PC7), TIM_Channel_2, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM8, IO_TAG(PC8), TIM_Channel_3, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM8, IO_TAG(PC9), TIM_Channel_4, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM2, IO_TAG(PA15), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM1, IO_TAG(PA8), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM3, IO_TAG(PC6), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR },
{ TIM8, IO_TAG(PC7), TIM_Channel_2, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO },
{ TIM4, IO_TAG(PB6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_LED }
};