All RPM sensors use the AP RPM library because the rpm sensor
is plugged into an AUX port on a Pixhawk, so we need to ensure that the AUX port is
configured as a GPIO pin to receive a signal input. Also, Ardupilot needs to know which
pin the RPM sensor is connected to.The RPM library in ArduPilot monitors the voltage of
the designated signal pin.With the latest version firmware of Ardupilot a maximum of two
RPM sensors can be used. For my project, I needed to active four rpm sensors, one for each
motor. This is the reason why I required tocustomize the firmware, particularly the RPM
library.
The following lines of programming code are those that were added to the firmware in order to active 4 rpm sensors. Basically, the tasks the code performs are:
• To active four pins, AUX pins as GPIO(Instead of two).
• To read and store the RPM sensor signals.
• To send the messages to the Full Parameters list, so that the user can configure the
parameters related with RPM sensors through MissionPlanner.
• To log RPM sensors readings into a micro sd card.
-
Modify libraries/AP RPM/AP RPM.h
-
Modify libraries/AP RPM/AP RPM.cpp as:
/*
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_Pin.h”
#include “RPM_SITL.h”
#include “RPM_EFI.h”
#include “RPM_HarmonicNotch.h”
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_RPM::var_info = {
// @Param: _TYPE
// @DisplayName: RPM type
// @Description: What type of RPM sensor is connected
// @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
// @User: Standard
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
// @User: Standard
AP_GROUPINFO("_SCALING", 1, AP_RPM, _scaling[0], 1.0f),
// @Param: _MAX
// @DisplayName: Maximum RPM
// @Description: Maximum RPM to report
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_MAX", 2, AP_RPM, _maximum, 100000),
// @Param: _MIN
// @DisplayName: Minimum RPM
// @Description: Minimum RPM to report
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_MIN", 3, AP_RPM, _minimum, 10),
// @Param: _MIN_QUAL
// @DisplayName: Minimum Quality
// @Description: Minimum data quality to be used
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("_MIN_QUAL", 4, AP_RPM, _quality_min, 0.5),
// @Param: _PIN
// @DisplayName: Input pin number
// @Description: Which pin to use
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("_PIN", 5, AP_RPM, _pin[0], 54),
#if RPM_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second RPM type
// @Description: What type of RPM sensor is connected
// @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
// @User: Advanced
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
// @User: Advanced
AP_GROUPINFO("2_SCALING", 11, AP_RPM, _scaling[1], 1.0f),
// @Param: 2_PIN
// @DisplayName: RPM2 input pin number
// @Description: Which pin to use
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("2_PIN", 12, AP_RPM, _pin[1], -1),
// @Param: 3_TYPE
// @DisplayName: Third RPM type
// @Description: What type of RPM sensor is connected
// @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
// @User: Advanced
AP_GROUPINFO("3_TYPE", 13, AP_RPM, _type[2], 0),
// @Param: 3_SCALING
// @DisplayName: RPM scaling
// @Description: Scaling factor between sensor reading and RPM.
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("3_SCALING", 14, AP_RPM, _scaling[2], 1.0f),
// @Param: 3_PIN
// @DisplayName: RPM3 input pin number
// @Description: Which pin to use
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("3_PIN", 15, AP_RPM, _pin[2], -1),
// @Param: 4_TYPE
// @DisplayName: Fourth RPM type
// @Description: What type of RPM sensor is connected
// @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch
// @User: Advanced
AP_GROUPINFO("4_TYPE", 16, AP_RPM, _type[3], 0),
// @Param: 4_SCALING
// @DisplayName: RPM scaling
// @Description: Scaling factor between sensor reading and RPM.
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO("4_SCALING", 17, AP_RPM, _scaling[3], 1.0f),
// @Param: 4_PIN
// @DisplayName: RPM4 input pin number
// @Description: Which pin to use
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("4_PIN", 18, AP_RPM, _pin[3], -1),
#endif
AP_GROUPEND
};
AP_RPM::AP_RPM(void)
{
AP_Param::setup_object_defaults(this, var_info);
if (_singleton != nullptr) {
AP_HAL::panic("AP_RPM must be singleton");
}
_singleton = this;
}
/*
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++) {
uint8_t type = _type[i];
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (type == RPM_TYPE_PWM) {
// PWM option same as PIN option, for upgrade
type = RPM_TYPE_PIN;
}
if (type == RPM_TYPE_PIN) {
drivers[i] = new AP_RPM_Pin(*this, i, state[i]);
}
#endif
#if EFI_ENABLED
if (type == RPM_TYPE_EFI) {
drivers[i] = new AP_RPM_EFI(*this, i, state[i]);
}
#endif
// include harmonic notch last
// this makes whatever process is driving the dynamic notch appear as an RPM value
if (type == RPM_TYPE_HNTCH) {
drivers[i] = new AP_RPM_HarmonicNotch(*this, i, state[i]);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type == RPM_TYPE_SITL) {
drivers[i] = new AP_RPM_SITL(*this, i, state[i]);
}
#endif
if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1; // num_instances is a high-water-mark
}
}
}
/*
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] != nullptr) {
if (_type[i] == RPM_TYPE_NONE) {
// allow user to disable an RPM sensor at runtime and force it to re-learn the quality if re-enabled.
state[i].signal_quality = 0;
continue;
}
drivers[i]->update();
}
}
}
/*
check if an instance is healthy
*/
bool AP_RPM::healthy(uint8_t instance) const
{
if (instance >= num_instances || _type[instance] == RPM_TYPE_NONE) {
return false;
}
// check that data quality is above minimum required
if (state[instance].signal_quality < _quality_min.get()) {
return false;
}
return true;
}
/*
check if an instance is activated
*/
bool AP_RPM::enabled(uint8_t instance) const
{
if (instance >= num_instances) {
return false;
}
// if no sensor type is selected, the sensor is not activated.
if (_type[instance] == RPM_TYPE_NONE) {
return false;
}
return true;
}
/*
get RPM value, return true on success
*/
bool AP_RPM::get_rpm(uint8_t instance, float &rpm_value) const
{
if (!healthy(instance)) {
return false;
}
rpm_value = state[instance].rate_rpm;
return true;
}
// singleton instance
AP_RPM *AP_RPM::_singleton;
namespace AP {
AP_RPM *rpm()
{
return AP_RPM::get_singleton();
}
}
- Modify libraries/AP RPM/AP Pin.cpp
/*
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>
#include “RPM_Pin.h”
#include <AP_HAL/GPIO.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_RPM_Pin::IrqState AP_RPM_Pin::irq_state[RPM_MAX_INSTANCES];
/*
open the sensor in constructor
*/
AP_RPM_Pin::AP_RPM_Pin(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) :
AP_RPM_Backend(_ap_rpm, instance, _state)
{
}
/*
handle interrupt on an instance
/
void AP_RPM_Pin::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
{
const uint32_t dt = timestamp - irq_state[state.instance].last_pulse_us;
irq_state[state.instance].last_pulse_us = timestamp;
// we don’t accept pulses less than 100us. Using an irq for such
// high RPM is too inaccurate, and it is probably just bounce of
// the signal which we should ignore
if (dt > 100 && dt < 10001000) {
irq_state[state.instance].dt_sum += dt;
irq_state[state.instance].dt_count++;
}
}
void AP_RPM_Pin::update(void)
{
if (last_pin != get_pin()) {
// detach from last pin
if (last_pin != (uint8_t)-1 &&
!hal.gpio->detach_interrupt(last_pin)) {
gcs().send_text(MAV_SEVERITY_WARNING, “RPM: Failed to detach from pin %u”, last_pin);
// ignore this failure or the user may be stuck
}
irq_state[state.instance].dt_count = 0;
irq_state[state.instance].dt_sum = 0;
// attach to new pin
last_pin = get_pin();
if (last_pin) {
hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT);
if (!hal.gpio->attach_interrupt(
last_pin,
FUNCTOR_BIND_MEMBER(&AP_RPM_Pin::irq_handler, void, uint8_t, bool, uint32_t),
AP_HAL::GPIO::INTERRUPT_RISING)) {
gcs().send_text(MAV_SEVERITY_WARNING, “RPM: Failed to attach to pin %u”, last_pin);
}
}
}
if (irq_state[state.instance].dt_count > 0) {
float dt_avg;
// disable interrupts to prevent race with irq_handler
void *irqstate = hal.scheduler->disable_interrupts_save();
dt_avg = irq_state[state.instance].dt_sum / irq_state[state.instance].dt_count;
irq_state[state.instance].dt_count = 0;
irq_state[state.instance].dt_sum = 0;
hal.scheduler->restore_interrupts(irqstate);
const float scaling = ap_rpm._scaling[state.instance];
float maximum = ap_rpm._maximum.get();
float minimum = ap_rpm._minimum.get();
float quality = 0;
float rpm = scaling * (1.0e6 / dt_avg) * 60;
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);
}
state.last_reading_ms = AP_HAL::millis();
} else {
quality = 0;
}
state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality);
}
// 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;
state.rate_rpm = 0;
}
}
- Modify ardupilot/libraries/AP Logger/LogFile.cpp
- Modify ardupilot/libraries/AP Logger/AP LogStructure.h

Hope it works now after all those changes. I will keep in touch.
Giovanna