AP_RPM: first version of RPM sensor driver
This commit is contained in:
Andrew Tridgell
parent
f2c1010501
commit
39c219d452
119
libraries/AP_RPM/AP_RPM.cpp
Normal file
119
libraries/AP_RPM/AP_RPM.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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; i<RPM_MAX_INSTANCES; i++) {
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
uint8_t type = _type[num_instances];
|
||||
uint8_t instance = num_instances;
|
||||
|
||||
if (type == RPM_TYPE_PX4_PWM) {
|
||||
state[instance].instance = instance;
|
||||
drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[instance]);
|
||||
}
|
||||
#endif
|
||||
if (drivers[i] != NULL) {
|
||||
// we loaded a driver for this instance, so it must be
|
||||
// present (although it may not be healthy)
|
||||
num_instances = i+1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update RPM state for all instances. This should be called by main loop
|
||||
*/
|
||||
void AP_RPM::update(void)
|
||||
{
|
||||
for (uint8_t i=0; i<num_instances; i++) {
|
||||
if (drivers[i] != NULL) {
|
||||
if (_type[i] == RPM_TYPE_NONE) {
|
||||
// allow user to disable a RPM sensor at runtime
|
||||
continue;
|
||||
}
|
||||
drivers[i]->update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
87
libraries/AP_RPM/AP_RPM.h
Normal file
87
libraries/AP_RPM/AP_RPM.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __RPM_H__
|
||||
#define __RPM_H__
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
// 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__
|
||||
29
libraries/AP_RPM/RPM_Backend.cpp
Normal file
29
libraries/AP_RPM/RPM_Backend.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_HAL.h>
|
||||
#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)
|
||||
{
|
||||
}
|
||||
42
libraries/AP_RPM/RPM_Backend.h
Normal file
42
libraries/AP_RPM/RPM_Backend.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef __AP_RPM_BACKEND_H__
|
||||
#define __AP_RPM_BACKEND_H__
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_HAL.h>
|
||||
#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__
|
||||
94
libraries/AP_RPM/RPM_PX4_PWM.cpp
Normal file
94
libraries/AP_RPM/RPM_PX4_PWM.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
#include "RPM_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 <math.h>
|
||||
|
||||
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
|
||||
40
libraries/AP_RPM/RPM_PX4_PWM.h
Normal file
40
libraries/AP_RPM/RPM_PX4_PWM.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
Loading…
Reference in New Issue
Block a user