Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +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 #ifdef USE_RANGEFINDER
// Store the raw rangefinder surface readout without applying tilt correction // Store the raw rangefinder surface readout without applying tilt correction
blackboxCurrent->surfaceRaw = rangefinderRead(); blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
#endif #endif
blackboxCurrent->rssi = rssi; 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); rescheduleTask(TASK_SELF, newDeadline);
} }
updatePositionEstimator_SurfaceTopic(currentTimeUs); /*
* Process raw rangefinder readout
*/
rangefinderProcess(calculateCosTiltAngle());
updatePositionEstimator_SurfaceTopic(currentTimeUs, rangefinderGetLatestAltitude());
} }
#endif #endif

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

@ -248,7 +248,7 @@ void navigationInit(void);
/* Position estimator update functions */ /* Position estimator update functions */
void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs); void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs); void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt);
/* Navigation system updates */ /* Navigation system updates */
void updateWaypointsAndNavigationMode(void); 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 * Read surface and update alt/vel topic
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available * 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) { if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
posEstimator.surface.alt = newSurfaceAlt; posEstimator.surface.alt = newSurfaceAlt;
posEstimator.surface.lastUpdateTime = currentTimeUs; 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. * 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) { if (rangefinder.dev.read) {
const int32_t distance = rangefinder.dev.read(); const int32_t distance = rangefinder.dev.read();
@ -213,31 +213,24 @@ int32_t rangefinderRead(void)
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE; rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
} }
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude); /**
* Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
return rangefinder.rawAltitude; * 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.
/** */
* Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
* 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) {
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE; rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
} else { } else {
rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle; rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
} }
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude); 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. * has never been called.
*/ */
int32_t rangefinderGetLatestAltitude(void) int32_t rangefinderGetLatestAltitude(void)
@ -245,6 +238,10 @@ int32_t rangefinderGetLatestAltitude(void)
return rangefinder.calculatedAltitude; return rangefinder.calculatedAltitude;
} }
int32_t rangefinderGetLatestRawAltitude(void) {
return rangefinder.rawAltitude;
}
bool rangefinderIsHealthy(void) bool rangefinderIsHealthy(void)
{ {
return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS; 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); bool rangefinderInit(void);
int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle);
int32_t rangefinderGetLatestAltitude(void); int32_t rangefinderGetLatestAltitude(void);
int32_t rangefinderGetLatestRawAltitude(void);
timeDelta_t rangefinderUpdate(void); timeDelta_t rangefinderUpdate(void);
int32_t rangefinderRead(void); void rangefinderProcess(float cosTiltAngle);
bool rangefinderIsHealthy(void); bool rangefinderIsHealthy(void);

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

@ -24,14 +24,13 @@
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { 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 }, { 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 }, { 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, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, { 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, 0, 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, 0, 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, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM2, 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, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, 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 } { TIM4, IO_TAG(PB6), TIM_Channel_1, 0, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_LED }
}; };