From 08173796d96761b5c316949505ce474a05101cec Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Jan 2019 09:45:11 +1100 Subject: [PATCH] APM_RPM: remove PX4 backend type --- libraries/AP_RPM/AP_RPM.cpp | 7 -- libraries/AP_RPM/RPM_PX4_PWM.cpp | 148 ------------------------------- libraries/AP_RPM/RPM_PX4_PWM.h | 41 --------- 3 files changed, 196 deletions(-) delete mode 100644 libraries/AP_RPM/RPM_PX4_PWM.cpp delete mode 100644 libraries/AP_RPM/RPM_PX4_PWM.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 630c9f5aee..47a741427e 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -14,7 +14,6 @@ */ #include "AP_RPM.h" -#include "RPM_PX4_PWM.h" #include "RPM_Pin.h" #include "RPM_SITL.h" @@ -107,16 +106,10 @@ void AP_RPM::init(void) for (uint8_t i=0; i. - */ - -#include - -#if (CONFIG_HAL_BOARD == HAL_BOARD_PX4) || ((CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN) && (!defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52))) -#include -#include "RPM_PX4_PWM.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#define PWM_LOGGING 0 - -extern const AP_HAL::HAL& hal; - -extern "C" { - int pwm_input_main(int, char **); -}; - -/* - open the sensor in constructor -*/ -AP_RPM_PX4_PWM::AP_RPM_PX4_PWM(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) : - AP_RPM_Backend(_ap_rpm, instance, _state) -{ -#if HAL_PX4_HAVE_PWM_INPUT - if (AP_BoardConfig::px4_start_driver(pwm_input_main, "pwm_input", "start")) { - hal.console->printf("started pwm_input driver\n"); - } -#endif - _fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - if (_fd == -1) { - hal.console->printf("Unable to open %s\n", PWMIN0_DEVICE_PATH); - return; - } - - // keep a queue of 5 samples to reduce noise by averaging - if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 5) != 0) { - hal.console->printf("Failed to setup RPM queue\n"); - close(_fd); - _fd = -1; - return; - } - - _resolution_usec = PWMIN_MINRPM_TO_RESOLUTION(((uint32_t)(ap_rpm._minimum[state.instance]+0.5f))); - ioctl(_fd, PWMINIOSRESOLUTION, _resolution_usec); - -#if PWM_LOGGING - _logfd = open("/fs/microsd/pwm.log", O_WRONLY|O_CREAT|O_TRUNC, 0644); -#endif -} - -/* - close the file descriptor -*/ -AP_RPM_PX4_PWM::~AP_RPM_PX4_PWM() -{ - if (_fd != -1) { - close(_fd); - _fd = -1; - } -} - -void AP_RPM_PX4_PWM::update(void) -{ - if (_fd == -1) { - return; - } - - uint32_t newres = PWMIN_MINRPM_TO_RESOLUTION(((uint32_t)(ap_rpm._minimum[state.instance]+0.5f))); - if (newres != _resolution_usec) { - ioctl(_fd, PWMINIOSRESOLUTION, newres); - _resolution_usec = newres; - } - - struct pwm_input_s pwm; - uint16_t count = 0; - const float scaling = ap_rpm._scaling[state.instance]; - float maximum = ap_rpm._maximum[state.instance]; - float minimum = ap_rpm._minimum[state.instance]; - float quality = 0; - - while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) { - // the px4 pwm_input driver reports the period in microseconds - if (pwm.period == 0) { - continue; - } - float rpm = scaling * (1.0e6f * 60) / pwm.period; - float filter_value = signal_quality_filter.get(); - state.rate_rpm = signal_quality_filter.apply(rpm); - if ((maximum <= 0 || rpm <= maximum) && (rpm >= minimum)) { - if (is_zero(filter_value)){ - quality = 0; - } else { - quality = 1 - constrain_float((fabsf(rpm-filter_value))/filter_value, 0.0, 1.0); - quality = powf(quality, 2.0); - } - count++; - } else { - quality = 0; - } - -#if PWM_LOGGING - if (_logfd != -1) { - dprintf(_logfd, "%u %u %u\n", - (unsigned)pwm.timestamp/1000, - (unsigned)pwm.period, - (unsigned)pwm.pulse_width); - } -#endif - - state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality); // simple LPF - } - - if (count != 0) { - state.last_reading_ms = AP_HAL::millis(); - } - - // assume we get readings at at least 1Hz, otherwise reset quality to zero - if (AP_HAL::millis() - state.last_reading_ms > 1000) { - state.signal_quality = 0; - } -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_RPM/RPM_PX4_PWM.h b/libraries/AP_RPM/RPM_PX4_PWM.h deleted file mode 100644 index 2e714f3aa2..0000000000 --- a/libraries/AP_RPM/RPM_PX4_PWM.h +++ /dev/null @@ -1,41 +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 . - */ -#pragma once - -#include "AP_RPM.h" -#include "RPM_Backend.h" -#include -#include - -class AP_RPM_PX4_PWM : public AP_RPM_Backend -{ -public: - // constructor - AP_RPM_PX4_PWM(AP_RPM &ranger, uint8_t instance, AP_RPM::RPM_State &_state); - - // destructor - ~AP_RPM_PX4_PWM(void); - - // update state - void update(void) override; - -private: - int _fd = -1; - int _logfd = -1; - uint64_t _last_timestamp; - uint32_t _resolution_usec = 1; - - ModeFilterFloat_Size5 signal_quality_filter {3}; -};