Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RSSI: support pwm input on gpio pin

This commit is contained in:
Randy Mackay 2018-07-28 13:35:10 +09:00
parent 4f6c50b79e
commit eb026eef98
2 changed files with 128 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
libraries/AP_RSSI/AP_RSSI.cpp
View File

@ -14,8 +14,10 @@
*/
#include <AP_RSSI/AP_RSSI.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <RC_Channel/RC_Channel.h>
#include <utility>
#include <utility>
#include <board_config.h>
extern const AP_HAL::HAL& hal;
@ -29,20 +31,21 @@ extern const AP_HAL::HAL& hal;
#define BOARD_RSSI_ANA_PIN_HIGH 5.0f
#endif
AP_RSSI::PWMState AP_RSSI::pwm_state;
const AP_Param::GroupInfo AP_RSSI::var_info[] = {
// @Param: TYPE
// @DisplayName: RSSI Type
// @Description: Radio Receiver RSSI type. If your radio receiver supports RSSI of some kind, set it here, then set its associated RSSI_XXXXX parameters, if any.
// @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue,3:ReceiverProtocol
// @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue,3:ReceiverProtocol,4:PWMInputPin
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 0, AP_RSSI, rssi_type, BOARD_RSSI_DEFAULT, AP_PARAM_FLAG_ENABLE),
// @Param: ANA_PIN
// @DisplayName: Receiver RSSI analog sensing pin
// @Description: This selects an analog pin where the receiver RSSI voltage will be read.
// @Values: 0:APM2 A0,1:APM2 A1,13:APM2 A13,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6,103:Pixhawk SBUS,15:Pixhawk2 ADC
// @DisplayName: Receiver RSSI sensing pin
// @Description: pin used to read the RSSI voltage or pwm value
// @Values: 0:APM2 A0,1:APM2 A1,13:APM2 A13,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6,15:Pixhawk2 ADC,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6,103:Pixhawk SBUS
// @User: Standard
AP_GROUPINFO("ANA_PIN", 1, AP_RSSI, rssi_analog_pin, BOARD_RSSI_ANA_PIN),
@ -72,8 +75,8 @@ const AP_Param::GroupInfo AP_RSSI::var_info[] = {
AP_GROUPINFO("CHANNEL", 4, AP_RSSI, rssi_channel, 0),
// @Param: CHAN_LOW
// @DisplayName: Receiver RSSI PWM low value
// @Description: This is the PWM value in microseconds that the radio receiver will put on the RSSI_CHANNEL when the signal strength is the weakest. Since some radio receivers put out inverted values from what you might otherwise expect, this isn't necessarily a lower value than RSSI_CHAN_HIGH.
// @DisplayName: RSSI PWM low value
// @Description: PWM value that the radio receiver will put on the RSSI_CHANNEL or RSSI_ANA_PIN when the signal strength is the weakest. Some radio receivers output inverted values so this value may be lower than RSSI_CHAN_HIGH
// @Units: PWM
// @Range: 0 2000
// @User: Standard
@ -81,7 +84,7 @@ const AP_Param::GroupInfo AP_RSSI::var_info[] = {
// @Param: CHAN_HIGH
// @DisplayName: Receiver RSSI PWM high value
// @Description: This is the PWM value in microseconds that the radio receiver will put on the RSSI_CHANNEL when the signal strength is the strongest. Since some radio receivers put out inverted values from what you might otherwise expect, this isn't necessarily a higher value than RSSI_CHAN_LOW.
// @Description: PWM value that the radio receiver will put on the RSSI_CHANNEL or RSSI_ANA_PIN when the signal strength is the strongest. Some radio receivers output inverted values so this value may be higher than RSSI_CHAN_LOW
// @Units: PWM
// @Range: 0 2000
// @User: Standard
@ -148,6 +151,9 @@ float AP_RSSI::read_receiver_rssi()
}
break;
}
case RssiType::RSSI_PWM_PIN:
receiver_rssi = read_pwm_pin_rssi();
break;
default :
receiver_rssi = 0.0f;
break;
@ -183,6 +189,49 @@ float AP_RSSI::read_channel_rssi()
return channel_rssi;
}
// read the PWM value from a pin
float AP_RSSI::read_pwm_pin_rssi()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// check if pin has changed and initialise gpio event callback
pwm_state.gpio = get_gpio(rssi_analog_pin);
if (pwm_state.gpio != pwm_state.last_gpio) {
// remove old gpio event callback if present
if (pwm_state.last_gpio != 0) {
stm32_gpiosetevent(pwm_state.last_gpio, false, false, false, nullptr);
pwm_state.last_gpio = 0;
}
// install interrupt handler on rising or falling edge of gpio
if (pwm_state.gpio != 0) {
stm32_gpiosetevent(pwm_state.gpio, true, true, false, irq_handler);
pwm_state.last_gpio = pwm_state.gpio;
}
}
// disable interrupts temporarily
irqstate_t istate = irqsave();
// check for timeout
float ret;
if ((pwm_state.last_reading_ms == 0) || (AP_HAL::millis() - pwm_state.last_reading_ms > 1000)) {
pwm_state.value = 0;
ret = 0;
} else {
// convert pwm value to rssi value
ret = scale_and_constrain_float_rssi(pwm_state.value, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
}
// restore interrupts
irqrestore(istate);
return ret;
#else
return 0.0f;
#endif
}
// Scale and constrain a float rssi value to 0.0 to 1.0 range
float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range)
{
@ -213,6 +262,58 @@ float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float lo
return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
}
// get gpio id from pin number
uint32_t AP_RSSI::get_gpio(uint8_t pin_number)
{
#ifdef GPIO_GPIO0_INPUT
switch (pin_number) {
case 50:
return GPIO_GPIO0_INPUT;
case 51:
return GPIO_GPIO1_INPUT;
case 52:
return GPIO_GPIO2_INPUT;
case 53:
return GPIO_GPIO3_INPUT;
case 54:
return GPIO_GPIO4_INPUT;
case 55:
return GPIO_GPIO5_INPUT;
}
#endif
return 0;
}
// interrupt handler for reading pwm value
int AP_RSSI::irq_handler(int irq, void *context)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// sanity check
if (pwm_state.gpio == 0) {
return 0;
}
// capture time
uint64_t now = AP_HAL::micros64();
// read value of pin
bool pin_high = stm32_gpioread(pwm_state.gpio);
// calculate pwm value
if (pin_high) {
pwm_state.pulse_start_us = now;
} else {
if (pwm_state.pulse_start_us != 0) {
pwm_state.value = now - pwm_state.pulse_start_us;
}
pwm_state.pulse_start_us = 0;
pwm_state.last_reading_ms = AP_HAL::millis();
}
#endif
return 0;
}
AP_RSSI *AP_RSSI::_s_instance = nullptr;
namespace AP {

21
libraries/AP_RSSI/AP_RSSI.h
View File

@ -25,7 +25,8 @@ public:
RSSI_DISABLED = 0,
RSSI_ANALOG_PIN = 1,
RSSI_RC_CHANNEL_VALUE = 2,
RSSI_RECEIVER = 3
RSSI_RECEIVER = 3,
RSSI_PWM_PIN = 4
};
AP_RSSI();
@ -73,14 +74,30 @@ private:
// a pin for reading the receiver RSSI voltage.
AP_HAL::AnalogSource *rssi_analog_source;
// PWM input
static struct PWMState {
uint32_t gpio; // gpio pin used for reading pwm
uint32_t last_gpio; // last gpio pin used for reading pwm (used to recognise change in pin assignment)
uint32_t value; // last calculated pwm value
uint32_t last_reading_ms; // system time of last read (used for health reporting)
uint64_t pulse_start_us; // system time of start of pulse
} pwm_state;
// read the RSSI value from an analog pin - returns float in range 0.0 to 1.0
float read_pin_rssi();
// read the RSSI value from a PWM value on a RC channel
float read_channel_rssi();
// Scale and constrain a float rssi value to 0.0 to 1.0 range
// read the PWM value from a pin
float read_pwm_pin_rssi();
// Scale and constrain a float rssi value to 0.0 to 1.0 range
float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
// PWM input handling
static uint32_t get_gpio(uint8_t pin_number);
static int irq_handler(int irq, void *context);
};
namespace AP {