From 59bfbbf8adb57119ec456cfe579169f1497c275f Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 22 Jul 2017 10:20:17 +0200 Subject: [PATCH 1/3] Rangefinder processing decoupled from reading --- src/main/blackbox/blackbox.c | 2 +- src/main/fc/fc_tasks.c | 10 +++++++++- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_pos_estimator.c | 5 +---- src/main/sensors/rangefinder.c | 8 +++++--- src/main/sensors/rangefinder.h | 5 ++--- 6 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 25e9112fa8..69b67be377 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0f0e2b1070..b1f5563234 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -184,7 +184,15 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) rescheduleTask(TASK_SELF, newDeadline); } - updatePositionEstimator_SurfaceTopic(currentTimeUs); + /* + * Process raw rangefinder readout + */ + rangefinderRead(); + + float newSurfaceAlt = rangefinderGetLatestRawAltitude(); + newSurfaceAlt = rangefinderCalculateAltitude(newSurfaceAlt, calculateCosTiltAngle()); + + updatePositionEstimator_SurfaceTopic(currentTimeUs, newSurfaceAlt); } #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index af74308e03..01de57cbe5 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 0f4b5803c2..dc86853b64 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -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; diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 3a5ccbd68e..9da73f9988 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -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 rangefinderRead(void) { if (rangefinder.dev.read) { const int32_t distance = rangefinder.dev.read(); @@ -214,8 +214,6 @@ int32_t rangefinderRead(void) } DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude); - - return rangefinder.rawAltitude; } /** @@ -245,6 +243,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; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 578d50642f..261be15cb6 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -48,11 +48,10 @@ 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 rangefinderRead(void); bool rangefinderIsHealthy(void); From fc144af1b4133173301fb10c5292c68064f42e39 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 22 Jul 2017 15:23:22 +0200 Subject: [PATCH 2/3] Review changes --- src/main/fc/fc_tasks.c | 9 +++------ src/main/sensors/rangefinder.c | 29 ++++++++++++----------------- src/main/sensors/rangefinder.h | 3 +-- 3 files changed, 16 insertions(+), 25 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b1f5563234..c4231de403 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -187,12 +187,9 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) /* * Process raw rangefinder readout */ - rangefinderRead(); - - float newSurfaceAlt = rangefinderGetLatestRawAltitude(); - newSurfaceAlt = rangefinderCalculateAltitude(newSurfaceAlt, calculateCosTiltAngle()); - - updatePositionEstimator_SurfaceTopic(currentTimeUs, newSurfaceAlt); + rangefinderProcess(calculateCosTiltAngle()); + + updatePositionEstimator_SurfaceTopic(currentTimeUs, rangefinderGetLatestAltitude()); } #endif diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 9da73f9988..3f57422e1e 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -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. */ -void rangefinderRead(void) +void rangefinderProcess(float cosTiltAngle) { if (rangefinder.dev.read) { const int32_t distance = rangefinder.dev.read(); @@ -213,29 +213,24 @@ void rangefinderRead(void) 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 - * 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) diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 261be15cb6..3d931fd2ec 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -48,10 +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); -void rangefinderRead(void); +void rangefinderProcess(float cosTiltAngle); bool rangefinderIsHealthy(void); From 7074db041c345b71b25cc1bdabaf726fbf57b42d Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sat, 22 Jul 2017 16:57:32 +0100 Subject: [PATCH 3/3] enable all motors on matek f405 --- src/main/target/MATEKF405/target.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 94b3766054..4a50dc6388 100755 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -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 } }; -