mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
APM_RangeFinder: remove PX4 backend type
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
08173796d9
commit
d71244dd23
@ -1,199 +0,0 @@
|
|||||||
/*
|
|
||||||
This program is free software: you can redistribute it and/or modify
|
|
||||||
it under the terms of the GNU General Public License as published by
|
|
||||||
the Free Software Foundation, either version 3 of the License, or
|
|
||||||
(at your option) any later version.
|
|
||||||
|
|
||||||
This program is distributed in the hope that it will be useful,
|
|
||||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
GNU General Public License for more details.
|
|
||||||
|
|
||||||
You should have received a copy of the GNU General Public License
|
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
||||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
||||||
#include "AP_RangeFinder_PX4_PWM.h"
|
|
||||||
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include <drivers/drv_pwm_input.h>
|
|
||||||
#include <drivers/drv_hrt.h>
|
|
||||||
#include <drivers/drv_sensor.h>
|
|
||||||
#include <uORB/topics/pwm_input.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <cmath>
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
|
||||||
|
|
||||||
extern "C" {
|
|
||||||
int pwm_input_main(int, char **);
|
|
||||||
};
|
|
||||||
|
|
||||||
/*
|
|
||||||
The constructor also initialises the rangefinder. Note that this
|
|
||||||
constructor is not called until detect() returns true, so we
|
|
||||||
already know that we should setup the rangefinder
|
|
||||||
*/
|
|
||||||
AP_RangeFinder_PX4_PWM::AP_RangeFinder_PX4_PWM(RangeFinder::RangeFinder_State &_state, AP_Int16 &powersave_range, float &_estimated_terrain_height) :
|
|
||||||
AP_RangeFinder_Backend(_state),
|
|
||||||
_powersave_range(powersave_range),
|
|
||||||
estimated_terrain_height(_estimated_terrain_height)
|
|
||||||
{
|
|
||||||
_fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
|
|
||||||
if (_fd == -1) {
|
|
||||||
hal.console->printf("Unable to open PX4 PWM rangefinder\n");
|
|
||||||
set_status(RangeFinder::RangeFinder_NotConnected);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// keep a queue of 20 samples
|
|
||||||
if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
|
|
||||||
hal.console->printf("Failed to setup range finder queue\n");
|
|
||||||
set_status(RangeFinder::RangeFinder_NotConnected);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// initialise to connected but no data
|
|
||||||
set_status(RangeFinder::RangeFinder_NoData);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
close the file descriptor
|
|
||||||
*/
|
|
||||||
AP_RangeFinder_PX4_PWM::~AP_RangeFinder_PX4_PWM()
|
|
||||||
{
|
|
||||||
if (_fd != -1) {
|
|
||||||
close(_fd);
|
|
||||||
}
|
|
||||||
set_status(RangeFinder::RangeFinder_NotConnected);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
see if the PX4 driver is available
|
|
||||||
*/
|
|
||||||
bool AP_RangeFinder_PX4_PWM::detect()
|
|
||||||
{
|
|
||||||
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && \
|
|
||||||
!defined(CONFIG_ARCH_BOARD_AEROFC_V1)
|
|
||||||
if (AP_BoardConfig::px4_start_driver(pwm_input_main, "pwm_input", "start")) {
|
|
||||||
hal.console->printf("started pwm_input driver\n");
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
|
|
||||||
if (fd == -1) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
close(fd);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_RangeFinder_PX4_PWM::update(void)
|
|
||||||
{
|
|
||||||
if (_fd == -1) {
|
|
||||||
set_status(RangeFinder::RangeFinder_NotConnected);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct pwm_input_s pwm;
|
|
||||||
float sum_cm = 0;
|
|
||||||
uint16_t count = 0;
|
|
||||||
const float scaling = state.scaling;
|
|
||||||
uint32_t now = AP_HAL::millis();
|
|
||||||
|
|
||||||
while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) {
|
|
||||||
// report the voltage as the pulse width, so we get the raw
|
|
||||||
// pulse widths in the log
|
|
||||||
state.voltage_mv = pwm.pulse_width;
|
|
||||||
|
|
||||||
state.last_reading_ms = now;
|
|
||||||
|
|
||||||
// setup for scaling in meters per millisecond
|
|
||||||
float _distance_cm = pwm.pulse_width * 0.1f * scaling + state.offset;
|
|
||||||
|
|
||||||
float distance_delta_cm = fabsf(_distance_cm - _last_sample_distance_cm);
|
|
||||||
_last_sample_distance_cm = _distance_cm;
|
|
||||||
|
|
||||||
if (distance_delta_cm > 100) {
|
|
||||||
// varying by more than 1m in a single sample, which means
|
|
||||||
// between 50 and 100m/s vertically - discard
|
|
||||||
_good_sample_count = 0;
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_good_sample_count > 1) {
|
|
||||||
count++;
|
|
||||||
sum_cm += _distance_cm;
|
|
||||||
_last_timestamp = pwm.timestamp;
|
|
||||||
} else {
|
|
||||||
_good_sample_count++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// if we haven't received a pulse for 1 second then we may need to
|
|
||||||
// reset the timer
|
|
||||||
int8_t stop_pin = state.stop_pin;
|
|
||||||
uint16_t settle_time_ms = (uint16_t)state.settle_time_ms;
|
|
||||||
|
|
||||||
if (stop_pin != -1 && out_of_range()) {
|
|
||||||
// we are above the power saving range. Disable the sensor
|
|
||||||
hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT);
|
|
||||||
hal.gpio->write(stop_pin, false);
|
|
||||||
set_status(RangeFinder::RangeFinder_NoData);
|
|
||||||
state.distance_cm = 0;
|
|
||||||
state.voltage_mv = 0;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// if we have not taken a reading in the last 0.2s set status to No Data
|
|
||||||
if (AP_HAL::micros64() - _last_timestamp >= 200000) {
|
|
||||||
set_status(RangeFinder::RangeFinder_NoData);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* if we haven't seen any pulses for 0.5s then the sensor is
|
|
||||||
probably dead. Try resetting it. Tests show the sensor takes
|
|
||||||
about 0.2s to boot, so 500ms offers some safety margin
|
|
||||||
*/
|
|
||||||
if (now - state.last_reading_ms > 500U && _disable_time_ms == 0) {
|
|
||||||
ioctl(_fd, SENSORIOCRESET, 0);
|
|
||||||
state.last_reading_ms = now;
|
|
||||||
|
|
||||||
// if a stop pin is configured then disable the sensor for the
|
|
||||||
// settle time
|
|
||||||
if (stop_pin != -1) {
|
|
||||||
hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT);
|
|
||||||
hal.gpio->write(stop_pin, false);
|
|
||||||
_disable_time_ms = now;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* the user can configure a settle time. This controls how
|
|
||||||
long the sensor is disabled for using the stop pin when it is
|
|
||||||
reset. This is used both to make sure the sensor is properly
|
|
||||||
reset, and also to allow for power management by running a low
|
|
||||||
duty cycle when it has no signal
|
|
||||||
*/
|
|
||||||
if (stop_pin != -1 && _disable_time_ms != 0 &&
|
|
||||||
(now - _disable_time_ms > settle_time_ms)) {
|
|
||||||
hal.gpio->write(stop_pin, true);
|
|
||||||
_disable_time_ms = 0;
|
|
||||||
state.last_reading_ms = now;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (count != 0) {
|
|
||||||
state.distance_cm = sum_cm / count;
|
|
||||||
|
|
||||||
// update range_valid state based on distance measured
|
|
||||||
update_status();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
/*
|
|
||||||
This program is free software: you can redistribute it and/or modify
|
|
||||||
it under the terms of the GNU General Public License as published by
|
|
||||||
the Free Software Foundation, either version 3 of the License, or
|
|
||||||
(at your option) any later version.
|
|
||||||
|
|
||||||
This program is distributed in the hope that it will be useful,
|
|
||||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
GNU General Public License for more details.
|
|
||||||
|
|
||||||
You should have received a copy of the GNU General Public License
|
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "RangeFinder.h"
|
|
||||||
#include "RangeFinder_Backend.h"
|
|
||||||
|
|
||||||
class AP_RangeFinder_PX4_PWM : public AP_RangeFinder_Backend
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
// constructor
|
|
||||||
AP_RangeFinder_PX4_PWM(RangeFinder::RangeFinder_State &_state, AP_Int16 &powersave_range, float &_estimated_terrain_height);
|
|
||||||
|
|
||||||
// destructor
|
|
||||||
~AP_RangeFinder_PX4_PWM(void);
|
|
||||||
|
|
||||||
// static detection function
|
|
||||||
static bool detect();
|
|
||||||
|
|
||||||
// update state
|
|
||||||
void update(void) override;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
|
||||||
return MAV_DISTANCE_SENSOR_UNKNOWN;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
int _fd;
|
|
||||||
uint64_t _last_timestamp;
|
|
||||||
uint32_t _disable_time_ms;
|
|
||||||
uint32_t _good_sample_count;
|
|
||||||
float _last_sample_distance_cm;
|
|
||||||
|
|
||||||
AP_Int16 &_powersave_range;
|
|
||||||
float &estimated_terrain_height;
|
|
||||||
|
|
||||||
};
|
|
||||||
@ -18,7 +18,6 @@
|
|||||||
#include "AP_RangeFinder_PulsedLightLRF.h"
|
#include "AP_RangeFinder_PulsedLightLRF.h"
|
||||||
#include "AP_RangeFinder_MaxsonarI2CXL.h"
|
#include "AP_RangeFinder_MaxsonarI2CXL.h"
|
||||||
#include "AP_RangeFinder_MaxsonarSerialLV.h"
|
#include "AP_RangeFinder_MaxsonarSerialLV.h"
|
||||||
#include "AP_RangeFinder_PX4_PWM.h"
|
|
||||||
#include "AP_RangeFinder_BBB_PRU.h"
|
#include "AP_RangeFinder_BBB_PRU.h"
|
||||||
#include "AP_RangeFinder_LightWareI2C.h"
|
#include "AP_RangeFinder_LightWareI2C.h"
|
||||||
#include "AP_RangeFinder_LightWareSerial.h"
|
#include "AP_RangeFinder_LightWareSerial.h"
|
||||||
@ -703,13 +702,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||||
case RangeFinder_TYPE_PX4_PWM:
|
|
||||||
if (AP_RangeFinder_PX4_PWM::detect()) {
|
|
||||||
drivers[instance] = new AP_RangeFinder_PX4_PWM(state[instance], _powersave_range, estimated_terrain_height);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
||||||
case RangeFinder_TYPE_PX4_PWM:
|
case RangeFinder_TYPE_PX4_PWM:
|
||||||
// to ease moving from PX4 to ChibiOS we'll lie a little about
|
// to ease moving from PX4 to ChibiOS we'll lie a little about
|
||||||
// the backend driver...
|
// the backend driver...
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user