diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp new file mode 100644 index 0000000000..3d17826c7f --- /dev/null +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -0,0 +1,119 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#include "AP_RPM.h" +#include "RPM_PX4_PWM.h" + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AP_RPM::var_info[] PROGMEM = { + // @Param: _TYPE + // @DisplayName: RPM type + // @Description: What type of RPM sensor is connected + // @Values: 0:None,1:PX4-PWM + AP_GROUPINFO("_TYPE", 0, AP_RPM, _type[0], 0), + + // @Param: _SCALING + // @DisplayName: RPM scaling + // @Description: Scaling factor between sensor reading and RPM. + // @Increment: 0.001 + AP_GROUPINFO("_SCALING", 1, AP_RPM, _scaling[0], 1.0f), + +#if RPM_MAX_INSTANCES > 1 + // @Param: 2_TYPE + // @DisplayName: Second RPM type + // @Description: What type of RPM sensor is connected + // @Values: 0:None,1:PX4-PWM + AP_GROUPINFO("2_TYPE", 10, AP_RPM, _type[1], 0), + + // @Param: 2_SCALING + // @DisplayName: RPM scaling + // @Description: Scaling factor between sensor reading and RPM. + // @Increment: 0.001 + AP_GROUPINFO("2_SCALING", 11, AP_RPM, _scaling[1], 1.0f), +#endif + + AP_GROUPEND +}; + +AP_RPM::AP_RPM(void) : + num_instances(0) +{ + AP_Param::setup_object_defaults(this, var_info); + + // init state and drivers + memset(state,0,sizeof(state)); + memset(drivers,0,sizeof(drivers)); +} + +/* + initialise the AP_RPM class. + */ +void AP_RPM::init(void) +{ + if (num_instances != 0) { + // init called a 2nd time? + return; + } + for (uint8_t i=0; iupdate(); + } + } +} + +/* + check if an instance is healthy + */ +bool AP_RPM::healthy(uint8_t instance) const +{ + if (instance >= num_instances) { + return false; + } + // assume we get readings at at least 1Hz + if (hal.scheduler->millis() - state[instance].last_reading_ms > 1000) { + return false; + } + return true; +} diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h new file mode 100644 index 0000000000..cbfebb7ec9 --- /dev/null +++ b/libraries/AP_RPM/AP_RPM.h @@ -0,0 +1,87 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#ifndef __RPM_H__ +#define __RPM_H__ + +#include +#include +#include +#include + +// Maximum number of RPM measurement instances available on this platform +#define RPM_MAX_INSTANCES 2 + +class AP_RPM_Backend; + +class AP_RPM +{ +public: + friend class AP_RPM_Backend; + + AP_RPM(void); + + // RPM driver types + enum RPM_Type { + RPM_TYPE_NONE = 0, + RPM_TYPE_PX4_PWM = 1 + }; + + // The RPM_State structure is filled in by the backend driver + struct RPM_State { + uint8_t instance; // the instance number of this RPM + float rate_rpm; // measured rate in revs per minute + uint32_t last_reading_ms; // time of last reading + }; + + // parameters for each instance + AP_Int8 _type[RPM_MAX_INSTANCES]; + AP_Float _scaling[RPM_MAX_INSTANCES]; + + static const struct AP_Param::GroupInfo var_info[]; + + // Return the number of rpm sensor instances + uint8_t num_sensors(void) const { + return num_instances; + } + + // detect and initialise any available rpm sensors + void init(void); + + // update state of all rpm sensors. Should be called from main loop + void update(void); + + /* + return RPM for a sensor. Return -1 if not healthy + */ + float get_rpm(uint8_t instance) const { + if (!healthy(instance)) { + return -1; + } + return state[instance].rate_rpm; + } + + bool healthy(uint8_t instance) const; + +private: + RPM_State state[RPM_MAX_INSTANCES]; + AP_RPM_Backend *drivers[RPM_MAX_INSTANCES]; + uint8_t num_instances:2; + + void detect_instance(uint8_t instance); + void update_instance(uint8_t instance); +}; +#endif // __RPM_H__ diff --git a/libraries/AP_RPM/RPM_Backend.cpp b/libraries/AP_RPM/RPM_Backend.cpp new file mode 100644 index 0000000000..cec72b74cf --- /dev/null +++ b/libraries/AP_RPM/RPM_Backend.cpp @@ -0,0 +1,29 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#include +#include +#include "AP_RPM.h" +#include "RPM_Backend.h" + +/* + base class constructor. +*/ +AP_RPM_Backend::AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) : + ap_rpm(_ap_rpm), + state(_state) +{ +} diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h new file mode 100644 index 0000000000..57241bfa44 --- /dev/null +++ b/libraries/AP_RPM/RPM_Backend.h @@ -0,0 +1,42 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#ifndef __AP_RPM_BACKEND_H__ +#define __AP_RPM_BACKEND_H__ + +#include +#include +#include "AP_RPM.h" + +class AP_RPM_Backend +{ +public: + // constructor. This incorporates initialisation as well. + AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state); + + // we declare a virtual destructor so that RPM drivers can + // override with a custom destructor if need be + virtual ~AP_RPM_Backend(void) {} + + // update the state structure. All backends must implement this. + virtual void update() = 0; + +protected: + + AP_RPM &ap_rpm; + AP_RPM::RPM_State &state; +}; +#endif // __AP_RPM_BACKEND_H__ diff --git a/libraries/AP_RPM/RPM_PX4_PWM.cpp b/libraries/AP_RPM/RPM_PX4_PWM.cpp new file mode 100644 index 0000000000..4ee145eef8 --- /dev/null +++ b/libraries/AP_RPM/RPM_PX4_PWM.cpp @@ -0,0 +1,94 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include "RPM_PX4_PWM.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + 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) +{ + _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; + } +} + +/* + 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; + } + + struct pwm_input_s pwm; + uint16_t count = 0; + const float scaling = ap_rpm._scaling[state.instance]; + float sum = 0; + + while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) { + // the px4 pwm_input driver reports the period in microseconds + if (pwm.period > 0) { + sum += (1.0e6f * 60) / pwm.period; + count++; + } + } + + if (count != 0) { + state.rate_rpm = scaling * sum / count; + state.last_reading_ms = hal.scheduler->millis(); + } +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_RPM/RPM_PX4_PWM.h b/libraries/AP_RPM/RPM_PX4_PWM.h new file mode 100644 index 0000000000..87d9446047 --- /dev/null +++ b/libraries/AP_RPM/RPM_PX4_PWM.h @@ -0,0 +1,40 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#ifndef AP_RPM_PX4_PWM_H +#define AP_RPM_PX4_PWM_H + +#include "AP_RPM.h" +#include "RPM_Backend.h" + +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); + +private: + int _fd = -1; + uint64_t _last_timestamp = 0; +}; + +#endif // AP_RPM_PX4_PWM_H