zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

APM_RangeFinder: remove PX4 backend type

mission-4.1.18
Peter Barker 6 years ago committed by Andrew Tridgell
parent
08173796d9
commit
d71244dd23
3 changed files with 1 additions and 258 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 199
      libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
  2. 51
      libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
  3. 9
      libraries/AP_RangeFinder/RangeFinder.cpp

199
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
Unescape View File

@ -1,199 +0,0 @@ @@ -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

51
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.h
Unescape View File

@ -1,51 +0,0 @@ @@ -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;
};

9
libraries/AP_RangeFinder/RangeFinder.cpp
Unescape View File

@ -18,7 +18,6 @@ @@ -18,7 +18,6 @@
#include "AP_RangeFinder_PulsedLightLRF.h"
#include "AP_RangeFinder_MaxsonarI2CXL.h"
#include "AP_RangeFinder_MaxsonarSerialLV.h"
#include "AP_RangeFinder_PX4_PWM.h"
#include "AP_RangeFinder_BBB_PRU.h"
#include "AP_RangeFinder_LightWareI2C.h"
#include "AP_RangeFinder_LightWareSerial.h"
@ -703,13 +702,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) @@ -703,13 +702,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
}
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
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
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
case RangeFinder_TYPE_PX4_PWM:
// to ease moving from PX4 to ChibiOS we'll lie a little about
// the backend driver...

Write Preview
Loading…
Cancel
Save