AP_Rangefinder: Remove stop pin usage

This commit is contained in:
Michael du Breuil 2019-09-02 20:19:00 -07:00 committed by Andrew Tridgell
parent 6c22faa585
commit c12652b325
4 changed files with 2 additions and 27 deletions

View File

@ -10,20 +10,6 @@ public:
virtual float read_latest() = 0;
virtual void set_pin(uint8_t p) = 0;
// optionally allow setting of a pin that stops the device from
// reading. This is needed for sonar devices where you have more
// than one sonar, and you want to stop them interfering with each
// other. It assumes that if held low the device is stopped, if
// held high the device starts reading.
virtual void set_stop_pin(uint8_t p) = 0;
// optionally allow a settle period in milliseconds. This is only
// used if a stop pin is set. If the settle period is non-zero
// then the analog input code will wait to get a reading for that
// number of milliseconds. Note that this will slow down the
// reading of analog inputs.
virtual void set_settle_time(uint16_t settle_time_ms) = 0;
// return a voltage from 0.0 to 5.0V, scaled
// against a reference voltage
virtual float voltage_average() = 0;

View File

@ -58,18 +58,12 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other.
// @Description: Digital pin that enables/disables rangefinder measurement for the pwm rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This is used to enable powersaving when out of range.
// @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Standard
AP_GROUPINFO("STOP_PIN", 8, AP_RangeFinder_Params, stop_pin, -1),
// @Param: SETTLE
// @DisplayName: Rangefinder settle time
// @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
// @Units: ms
// @Increment: 1
// @User: Standard
AP_GROUPINFO("SETTLE", 9, AP_RangeFinder_Params, settle_time_ms, 0),
// 9 was SETTLE
// @Param: RMETRIC
// @DisplayName: Ratiometric

View File

@ -18,7 +18,6 @@ public:
AP_Int8 ratiometric;
AP_Int16 powersave_range;
AP_Int8 stop_pin;
AP_Int16 settle_time_ms;
AP_Float scaling;
AP_Float offset;
AP_Int8 function;

View File

@ -41,8 +41,6 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_st
set_status(RangeFinder::RangeFinder_NotConnected);
return;
}
source->set_stop_pin((uint8_t)_params.stop_pin);
source->set_settle_time((uint16_t)_params.settle_time_ms);
set_status(RangeFinder::RangeFinder_NoData);
}
@ -71,8 +69,6 @@ void AP_RangeFinder_analog::update_voltage(void)
}
// cope with changed settings
source->set_pin(params.pin);
source->set_stop_pin((uint8_t)params.stop_pin);
source->set_settle_time((uint16_t)params.settle_time_ms);
if (params.ratiometric) {
state.voltage_mv = source->voltage_average_ratiometric() * 1000U;
} else {