diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d50a556bd1..eecfc9ef56 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -214,7 +214,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"AirSpeed", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_PITOT}, #endif #ifdef USE_RANGEFINDER - {"sonarRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR}, + {"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SURFACE}, #endif {"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI}, @@ -353,7 +353,7 @@ typedef struct blackboxMainState_s { int16_t magADC[XYZ_AXIS_COUNT]; #endif #ifdef USE_RANGEFINDER - int32_t sonarRaw; + int32_t surfaceRaw; #endif uint16_t rssi; #ifdef NAV_BLACKBOX @@ -499,7 +499,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC: return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC; - case FLIGHT_LOG_FIELD_CONDITION_SONAR: + case FLIGHT_LOG_FIELD_CONDITION_SURFACE: #ifdef USE_RANGEFINDER return sensors(SENSOR_RANGEFINDER); #else @@ -635,8 +635,8 @@ static void writeIntraframe(void) #endif #ifdef USE_RANGEFINDER - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) { - blackboxWriteSignedVB(blackboxCurrent->sonarRaw); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SURFACE)) { + blackboxWriteSignedVB(blackboxCurrent->surfaceRaw); } #endif @@ -804,8 +804,8 @@ static void writeInterframe(void) #endif #ifdef USE_RANGEFINDER - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) { - deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw; + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SURFACE)) { + deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw; } #endif @@ -1145,8 +1145,8 @@ static void loadMainState(timeUs_t currentTimeUs) #endif #ifdef USE_RANGEFINDER - // Store the raw sonar value without applying tilt correction - blackboxCurrent->sonarRaw = rangefinderRead(); + // Store the raw rangefinder surface readout without applying tilt correction + blackboxCurrent->surfaceRaw = rangefinderRead(); #endif blackboxCurrent->rssi = rssi; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 95d48b906c..b339a79a4b 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -39,7 +39,7 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_PITOT, FLIGHT_LOG_FIELD_CONDITION_VBAT, FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC, - FLIGHT_LOG_FIELD_CONDITION_SONAR, + FLIGHT_LOG_FIELD_CONDITION_SURFACE, FLIGHT_LOG_FIELD_CONDITION_RSSI, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1e7e1db976..aa18d95635 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -155,10 +155,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif #ifdef USE_RANGEFINDER_HCSR04 - if (init->useSonar && + if (init->useTriggerRangefinder && ( - timerHardwarePtr->tag == init->sonarIOConfig.triggerTag || - timerHardwarePtr->tag == init->sonarIOConfig.echoTag + timerHardwarePtr->tag == init->rangefinderIOConfig.triggerTag || + timerHardwarePtr->tag == init->rangefinderIOConfig.echoTag )) { addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ac9a71953b..423a1ac1f4 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -44,10 +44,10 @@ #define MAX_INPUTS 8 -typedef struct sonarIOConfig_s { +typedef struct rangefinderIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; -} sonarIOConfig_t; +} rangefinderIOConfig_t; typedef enum { PLATFORM_MULTIROTOR = 0, @@ -72,7 +72,7 @@ typedef struct drv_pwm_config_s { bool useSoftSerial; bool useLEDStrip; #ifdef USE_RANGEFINDER - bool useSonar; + bool useTriggerRangefinder; #endif #ifdef USE_SERVOS bool useServoOutputs; @@ -84,7 +84,7 @@ typedef struct drv_pwm_config_s { uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. - sonarIOConfig_t sonarIOConfig; + rangefinderIOConfig_t rangefinderIOConfig; } drv_pwm_config_t; typedef enum { diff --git a/src/main/drivers/rangefinder_hcsr04.c b/src/main/drivers/rangefinder_hcsr04.c index 9144f2244c..994e41963e 100644 --- a/src/main/drivers/rangefinder_hcsr04.c +++ b/src/main/drivers/rangefinder_hcsr04.c @@ -147,7 +147,7 @@ int32_t hcsr04_get_distance(void) return lastCalculatedDistance; } -bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins) +bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins) { bool detected = false; @@ -160,8 +160,8 @@ bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonar RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); #endif - triggerIO = IOGetByTag(sonarHardwarePins->triggerTag); - echoIO = IOGetByTag(sonarHardwarePins->echoTag); + triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag); + echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); if (IOGetOwner(triggerIO) != OWNER_FREE) { addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(triggerIO), OWNER_RANGEFINDER); diff --git a/src/main/drivers/rangefinder_hcsr04.h b/src/main/drivers/rangefinder_hcsr04.h index 5cc3983008..5e08760808 100644 --- a/src/main/drivers/rangefinder_hcsr04.h +++ b/src/main/drivers/rangefinder_hcsr04.h @@ -21,4 +21,4 @@ #define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70 -bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins); +bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * rangefinderHardwarePins); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 974d756977..c95cce8789 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3120,7 +3120,7 @@ static void cliStatus(char *cmdline) } cliPrint("\r\n"); - cliPrintf("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, SONAR=%s, GPS=%s\r\n", + cliPrintf("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, GPS=%s\r\n", hardwareSensorStatusNames[getHwGyroStatus()], hardwareSensorStatusNames[getHwAccelerometerStatus()], hardwareSensorStatusNames[getHwCompassStatus()], diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 24a0481850..5aa181ce12 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -252,11 +252,11 @@ void init(void) #ifdef USE_RANGEFINDER_HCSR04 // HC-SR04 has a dedicated connection to FC and require two pins if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) { - const rangefinderHardwarePins_t *sonarHardware = sonarGetHardwarePins(); - if (sonarHardware) { - pwm_params.useSonar = true; - pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; - pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; + const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); + if (rangefinderHardwarePins) { + pwm_params.useTriggerRangefinder = true; + pwm_params.rangefinderIOConfig.triggerTag = rangefinderHardwarePins->triggerTag; + pwm_params.rangefinderIOConfig.echoTag = rangefinderHardwarePins->echoTag; } } #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 1cd58db5a4..d9cf6881dd 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -162,7 +162,7 @@ #define MSP_VOLTAGE_METER_CONFIG 56 #define MSP_SET_VOLTAGE_METER_CONFIG 57 -#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] +#define MSP_SONAR_ALTITUDE 58 //out message get surface altitude [cm] #define MSP_PID_CONTROLLER 59 #define MSP_SET_PID_CONTROLLER 60 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index da0e56e59c..1cb7005cac 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -995,7 +995,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if (navConfig()->general.flags.rth_allow_landing) { float descentVelLimited = 0; - // A safeguard - if sonar is available and it is reading < 50cm altitude - drop to low descend speed + // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index aff2671872..0f4b5803c2 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -76,7 +76,7 @@ #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout -#define INAV_SONAR_TIMEOUT_MS 300 // Sonar timeout (missed 3 readings in a row) +#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row) #define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data @@ -111,7 +111,7 @@ typedef struct { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) float alt; // Raw altitude measurement (cm) -} navPositionEstimatorSONAR_t; +} navPositionEstimatorSURFACE_t; typedef struct { timeUs_t lastUpdateTime; // Last update time (us) @@ -148,7 +148,7 @@ typedef struct { // Data sources navPositionEstimatorGPS_t gps; navPositionEstimatorBARO_t baro; - navPositionEstimatorSONAR_t sonar; + navPositionEstimatorSURFACE_t surface; navPositionEstimatorPITOT_t pitot; // IMU data @@ -471,17 +471,17 @@ static void updatePitotTopic(timeUs_t currentTimeUs) #ifdef USE_RANGEFINDER /** - * Read sonar 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 */ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs) { - float newSonarAlt = rangefinderRead(); - newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle()); + float newSurfaceAlt = rangefinderRead(); + newSurfaceAlt = rangefinderCalculateAltitude(newSurfaceAlt, calculateCosTiltAngle()); - if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_surface_altitude) { - posEstimator.sonar.alt = newSonarAlt; - posEstimator.sonar.lastUpdateTime = currentTimeUs; + if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { + posEstimator.surface.alt = newSurfaceAlt; + posEstimator.surface.lastUpdateTime = currentTimeUs; } } #endif @@ -600,7 +600,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv); // EPV is checked later bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS)); - bool isSonarValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS)); + bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS)); /* Do some preparations to data */ if (isBaroValid) { @@ -626,7 +626,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) /* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */ bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && - ((isSonarValid && posEstimator.sonar.alt < 20.0f && posEstimator.state.isBaroGroundValid) || + ((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || (isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); #if defined(NAV_GPS_GLITCH_DETECTION) @@ -812,12 +812,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) #ifdef USE_RANGEFINDER posEstimator.est.surface = posEstimator.est.surface + posEstimator.est.surfaceVel * dt; - if (isSonarValid) { - const float sonarResidual = posEstimator.sonar.alt - posEstimator.est.surface; - const float bellCurveScaler = scaleRangef(bellCurve(sonarResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f); + if (isSurfaceValid) { + const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.surface; + const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f); - posEstimator.est.surface += sonarResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * dt; - posEstimator.est.surfaceVel += sonarResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * dt; + posEstimator.est.surface += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * dt; + posEstimator.est.surfaceVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * dt; posEstimator.est.surfaceValid = true; } else { @@ -903,7 +903,7 @@ void initializePositionEstimator(void) posEstimator.gps.lastUpdateTime = 0; posEstimator.baro.lastUpdateTime = 0; - posEstimator.sonar.lastUpdateTime = 0; + posEstimator.surface.lastUpdateTime = 0; posEstimator.est.surface = 0; posEstimator.est.surfaceVel = 0; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2207c8f429..bce5111c03 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -77,7 +77,7 @@ typedef struct navigationFlags_s { // Behaviour modifiers bool isGCSAssistedNavigationEnabled; // Does iNav accept WP#255 - follow-me etc. bool isGCSAssistedNavigationReset; // GCS control was disabled - indicate that so code could take action accordingly - bool isTerrainFollowEnabled; // Does iNav use sonar for terrain following (adjusting baro altitude target according to sonar readings) + bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; } navigationFlags_t; diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 55e513a52b..6fadf76494 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -59,7 +59,7 @@ PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, .rangefinder_hardware = RANGEFINDER_NONE, ); -const rangefinderHardwarePins_t * sonarGetHardwarePins(void) +const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void) { static rangefinderHardwarePins_t rangefinderHardwarePins; @@ -95,8 +95,8 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar case RANGEFINDER_HCSR04: #ifdef USE_RANGEFINDER_HCSR04 { - const rangefinderHardwarePins_t *sonarHardwarePins = sonarGetHardwarePins(); - if (hcsr04Detect(dev, sonarHardwarePins)) { // FIXME: Do actual detection if HC-SR04 is plugged in + const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); + if (hcsr04Detect(dev, rangefinderHardwarePins)) { // FIXME: Do actual detection if HC-SR04 is plugged in rangefinderHardware = RANGEFINDER_HCSR04; rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_TASK_PERIOD_MS)); } diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 265391b79a..2e43f5d035 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -43,7 +43,7 @@ typedef struct rangefinder_s { extern rangefinder_t rangefinder; -const rangefinderHardwarePins_t * sonarGetHardwarePins(void); +const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void); bool rangefinderInit(void); diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index b1aac39d62..94f1f5ae28 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -28,8 +28,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #else { TIM4, IO_TAG(PB6), TIM_Channel_1, 0, IOCFG_IPD, TIM_USE_PWM }, // S1_IN #endif - { TIM3, IO_TAG(PB5), TIM_Channel_2, 0, IOCFG_IPD, TIM_USE_PWM }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_IPD, TIM_USE_PWM }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB5), TIM_Channel_2, 0, IOCFG_IPD, TIM_USE_PWM }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / HC-SR04 trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, 0, IOCFG_IPD, TIM_USE_PWM }, // S3_IN - SoftSerial RX / HC-SR04 echo / RSSI ADC { TIM3, IO_TAG(PB1), TIM_Channel_4, 0, IOCFG_IPD, TIM_USE_PWM }, // S4_IN - Current { TIM2, IO_TAG(PA0), TIM_Channel_1, 0, IOCFG_IPD, TIM_USE_PWM }, // S5_IN - Vbattery #ifdef CC3D_PPM1 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 0b77db6f32..b579eb46c4 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -98,8 +98,8 @@ // RC1 GND // RC2 power // RC3 PB6/TIM4 unused -// RC4 PB5/TIM3 SCK / softserial1 TX / sonar trigger -// RC5 PB0/TIM3 MISO / softserial1 RX / sonar echo / RSSI ADC +// RC4 PB5/TIM3 SCK / softserial1 TX / HC-SR04 trigger +// RC5 PB0/TIM3 MISO / softserial1 RX / HC-SR04 echo / RSSI ADC // RC6 PB1/TIM3 MOSI / current // RC7 PA0/TIM2 CSN / battery voltage // RC8 PA1/TIM2 CE / RX_PPM diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index cb9a01151a..6238f40d13 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -24,6 +24,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PE13), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S7_OUT { TIM1, IO_TAG(PE14), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO}, // S8_OUT - { TIM9, IO_TAG(PE6), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_ANY }, // sonar echo if needed + { TIM9, IO_TAG(PE6), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_ANY }, // HC-SR04 echo if needed }; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 48b98de6ff..ae5bd8ce7b 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -151,8 +151,8 @@ // RC6 PA3/TIM2 USART2 RX // RC7 PA6/TIM3 CSN / softserial1 RX / LED_STRIP // RC8 PA7/TIM3 SCK / softserial1 TX -// RC9 PB0/TIM3 MISO / softserial2 RX / sonar trigger -// RC10 PB1/TIM3 MOSI /softserial2 TX / sonar echo / current +// RC9 PB0/TIM3 MISO / softserial2 RX / HC-SR04 trigger +// RC10 PB1/TIM3 MOSI /softserial2 TX / HC-SR04 echo / current // Nordic Semiconductor uses 'CSN', STM uses 'NSS' #define RX_CE_PIN PA1 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c8b9e4c269..e1b89ea422 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -397,7 +397,7 @@ void mavlinkSendHUDAndHeartbeat(void) mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; #elif defined(GPS) if (sensors(SENSOR_GPS)) { - // No sonar or baro, just display altitude above MLS + // No surface or baro, just display altitude above MLS mavAltitude = gpsSol.llh.alt; } #endif @@ -411,7 +411,7 @@ void mavlinkSendHUDAndHeartbeat(void) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), - // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate) + // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second mavClimbRate);