2015-08-28 02:59:17 -03:00
/*
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_RSSI/AP_RSSI.h>
2018-08-12 21:53:03 -03:00
# include <GCS_MAVLink/GCS.h>
2018-04-03 23:18:50 -03:00
# include <RC_Channel/RC_Channel.h>
2018-08-12 21:53:03 -03:00
2018-07-28 01:35:10 -03:00
# include <utility>
2015-08-28 02:59:17 -03:00
extern const AP_HAL : : HAL & hal ;
2018-07-31 02:57:24 -03:00
# ifndef BOARD_RSSI_DEFAULT
2016-04-13 22:09:17 -03:00
# define BOARD_RSSI_DEFAULT 0
2018-07-31 02:57:24 -03:00
# endif
# ifndef BOARD_RSSI_ANA_PIN
2016-04-13 22:09:17 -03:00
# define BOARD_RSSI_ANA_PIN 0
2018-07-31 02:57:24 -03:00
# endif
# ifndef BOARD_RSSI_ANA_PIN_HIGH
2016-04-13 22:09:17 -03:00
# define BOARD_RSSI_ANA_PIN_HIGH 5.0f
# endif
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_RSSI : : var_info [ ] = {
2015-09-16 03:58:38 -03:00
2015-09-25 09:00:22 -03:00
// @Param: TYPE
2015-08-28 02:59:17 -03:00
// @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.
2020-06-03 23:02:56 -03:00
// @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue,3:ReceiverProtocol,4:PWMInputPin,5:TelemetryRadioRSSI
2015-08-28 02:59:17 -03:00
// @User: Standard
2018-04-10 01:59:03 -03:00
AP_GROUPINFO_FLAGS ( " TYPE " , 0 , AP_RSSI , rssi_type , BOARD_RSSI_DEFAULT , AP_PARAM_FLAG_ENABLE ) ,
2015-09-16 03:58:38 -03:00
2015-09-25 09:00:22 -03:00
// @Param: ANA_PIN
2018-07-28 01:35:10 -03:00
// @DisplayName: Receiver RSSI sensing pin
2018-07-29 21:11:56 -03:00
// @Description: Pin used to read the RSSI voltage or PWM value
2020-03-23 10:08:54 -03:00
// @Values: 8:V5 Nano,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6,103:Pixhawk SBUS
2015-08-28 02:59:17 -03:00
// @User: Standard
2016-04-13 22:09:17 -03:00
AP_GROUPINFO ( " ANA_PIN " , 1 , AP_RSSI , rssi_analog_pin , BOARD_RSSI_ANA_PIN ) ,
2015-08-28 02:59:17 -03:00
2015-09-25 09:00:22 -03:00
// @Param: PIN_LOW
2018-07-28 01:31:08 -03:00
// @DisplayName: RSSI pin's lowest voltage
// @Description: RSSI pin's voltage received on the RSSI_ANA_PIN when the signal strength is the weakest. Some radio receivers put out inverted values so this value may be higher than RSSI_PIN_HIGH
2017-05-02 10:47:27 -03:00
// @Units: V
2015-08-28 02:59:17 -03:00
// @Increment: 0.01
// @Range: 0 5.0
// @User: Standard
AP_GROUPINFO ( " PIN_LOW " , 2 , AP_RSSI , rssi_analog_pin_range_low , 0.0f ) ,
2015-09-25 09:00:22 -03:00
// @Param: PIN_HIGH
2018-07-28 01:31:08 -03:00
// @DisplayName: RSSI pin's highest voltage
// @Description: RSSI pin's voltage received on the RSSI_ANA_PIN when the signal strength is the strongest. Some radio receivers put out inverted values so this value may be lower than RSSI_PIN_LOW
2017-05-02 10:47:27 -03:00
// @Units: V
2015-08-28 02:59:17 -03:00
// @Increment: 0.01
// @Range: 0 5.0
// @User: Standard
2016-04-13 22:09:17 -03:00
AP_GROUPINFO ( " PIN_HIGH " , 3 , AP_RSSI , rssi_analog_pin_range_high , BOARD_RSSI_ANA_PIN_HIGH ) ,
2015-09-16 03:58:38 -03:00
2015-09-25 09:00:22 -03:00
// @Param: CHANNEL
2015-08-28 02:59:17 -03:00
// @DisplayName: Receiver RSSI channel number
2016-03-29 12:16:42 -03:00
// @Description: The channel number where RSSI will be output by the radio receiver (5 and above).
2018-07-28 01:31:08 -03:00
// @Range: 0 16
2015-08-28 02:59:17 -03:00
// @User: Standard
AP_GROUPINFO ( " CHANNEL " , 4 , AP_RSSI , rssi_channel , 0 ) ,
2015-09-16 03:58:38 -03:00
2015-09-25 09:00:22 -03:00
// @Param: CHAN_LOW
2018-07-28 01:35:10 -03:00
// @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
2017-05-02 10:47:27 -03:00
// @Units: PWM
2015-08-28 02:59:17 -03:00
// @Range: 0 2000
// @User: Standard
AP_GROUPINFO ( " CHAN_LOW " , 5 , AP_RSSI , rssi_channel_low_pwm_value , 1000 ) ,
2015-09-16 03:58:38 -03:00
2015-09-25 09:00:22 -03:00
// @Param: CHAN_HIGH
2015-08-28 02:59:17 -03:00
// @DisplayName: Receiver RSSI PWM high value
2018-07-28 01:35:10 -03:00
// @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
2017-05-02 10:47:27 -03:00
// @Units: PWM
2015-08-28 02:59:17 -03:00
// @Range: 0 2000
// @User: Standard
AP_GROUPINFO ( " CHAN_HIGH " , 6 , AP_RSSI , rssi_channel_high_pwm_value , 2000 ) ,
2015-09-16 03:58:38 -03:00
2015-08-28 02:59:17 -03:00
AP_GROUPEND
} ;
// Public
// ------
// constructor
AP_RSSI : : AP_RSSI ( )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2019-02-10 01:04:49 -04:00
if ( _singleton ) {
2018-05-02 07:16:35 -03:00
AP_HAL : : panic ( " Too many RSSI sensors " ) ;
}
2019-02-10 01:04:49 -04:00
_singleton = this ;
2015-08-28 02:59:17 -03:00
}
// destructor
AP_RSSI : : ~ AP_RSSI ( void )
{
}
2018-05-02 07:16:35 -03:00
/*
* Get the AP_RSSI singleton
*/
2019-02-10 01:04:49 -04:00
AP_RSSI * AP_RSSI : : get_singleton ( )
2018-05-02 07:16:35 -03:00
{
2019-02-10 01:04:49 -04:00
return _singleton ;
2018-05-02 07:16:35 -03:00
}
2015-08-28 02:59:17 -03:00
// Initialize the rssi object and prepare it for use
void AP_RSSI : : init ( )
{
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
rssi_analog_source = hal . analogin - > channel ( ANALOG_INPUT_NONE ) ;
}
// Read the receiver RSSI value as a float 0.0f - 1.0f.
// 0.0 represents weakest signal, 1.0 represents maximum signal.
float AP_RSSI : : read_receiver_rssi ( )
{
2019-04-05 20:12:25 -03:00
switch ( RssiType ( rssi_type . get ( ) ) ) {
case RssiType : : TYPE_DISABLED :
return 0.0f ;
case RssiType : : ANALOG_PIN :
return read_pin_rssi ( ) ;
case RssiType : : RC_CHANNEL_VALUE :
return read_channel_rssi ( ) ;
case RssiType : : RECEIVER : {
2018-04-03 23:18:50 -03:00
int16_t rssi = RC_Channels : : get_receiver_rssi ( ) ;
2017-06-20 00:13:15 -03:00
if ( rssi ! = - 1 ) {
2019-04-05 20:12:25 -03:00
return rssi / 255.0 ;
2017-06-20 00:13:15 -03:00
}
2019-04-05 20:12:25 -03:00
return 0.0f ;
2017-06-20 00:13:15 -03:00
}
2019-04-05 20:12:25 -03:00
case RssiType : : PWM_PIN :
return read_pwm_pin_rssi ( ) ;
2020-06-03 23:02:56 -03:00
case RssiType : : TELEMETRY_RADIO_RSSI :
return read_telemetry_radio_rssi ( ) ;
2019-04-05 20:12:25 -03:00
}
// should never get to here
return 0.0f ;
2015-08-28 02:59:17 -03:00
}
// Read the receiver RSSI value as an 8-bit integer
// 0 represents weakest signal, 255 represents maximum signal.
uint8_t AP_RSSI : : read_receiver_rssi_uint8 ( )
{
return read_receiver_rssi ( ) * 255 ;
}
// Private
// -------
// read the RSSI value from an analog pin - returns float in range 0.0 to 1.0
float AP_RSSI : : read_pin_rssi ( )
{
rssi_analog_source - > set_pin ( rssi_analog_pin ) ;
float current_analog_voltage = rssi_analog_source - > voltage_average ( ) ;
2015-09-16 03:58:38 -03:00
2015-08-28 02:59:17 -03:00
return scale_and_constrain_float_rssi ( current_analog_voltage , rssi_analog_pin_range_low , rssi_analog_pin_range_high ) ;
}
// read the RSSI value from a PWM value on a RC channel
float AP_RSSI : : read_channel_rssi ( )
{
2018-10-11 20:44:00 -03:00
RC_Channel * c = rc ( ) . channel ( rssi_channel - 1 ) ;
if ( c = = nullptr ) {
2018-04-26 09:00:09 -03:00
return 0.0f ;
}
2018-10-11 20:44:00 -03:00
uint16_t rssi_channel_value = c - > get_radio_in ( ) ;
2015-08-28 02:59:17 -03:00
float channel_rssi = scale_and_constrain_float_rssi ( rssi_channel_value , rssi_channel_low_pwm_value , rssi_channel_high_pwm_value ) ;
return channel_rssi ;
}
2018-08-12 21:53:03 -03:00
// read the PWM value from a pin
float AP_RSSI : : read_pwm_pin_rssi ( )
{
// check if pin has changed and configure interrupt handlers if required:
2020-09-01 02:45:15 -03:00
if ( ! pwm_state . pwm_source . set_pin ( rssi_analog_pin , " RSSI " ) ) {
2018-08-12 21:53:03 -03:00
// disabled (either by configuration or failure to attach interrupt)
return 0.0f ;
2018-07-28 01:35:10 -03:00
}
2020-09-01 02:45:15 -03:00
uint16_t pwm_us = pwm_state . pwm_source . get_pwm_us ( ) ;
2018-08-12 21:53:03 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
2020-09-01 02:45:15 -03:00
if ( pwm_us = = 0 ) {
2018-08-12 21:53:03 -03:00
// no reading; check for timeout:
if ( now - pwm_state . last_reading_ms > 1000 ) {
// no reading for a second - something is broken
pwm_state . rssi_value = 0.0f ;
}
} else {
// a new reading - convert pwm value to rssi value
2020-09-01 02:45:15 -03:00
pwm_state . rssi_value = scale_and_constrain_float_rssi ( pwm_us , rssi_channel_low_pwm_value , rssi_channel_high_pwm_value ) ;
2018-08-12 21:53:03 -03:00
pwm_state . last_reading_ms = now ;
}
2018-07-28 01:35:10 -03:00
2018-08-12 21:53:03 -03:00
return pwm_state . rssi_value ;
2018-07-28 01:35:10 -03:00
}
2020-06-03 23:02:56 -03:00
float AP_RSSI : : read_telemetry_radio_rssi ( )
{
return GCS_MAVLINK : : telemetry_radio_rssi ( ) ;
}
2015-08-28 02:59:17 -03:00
// 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 )
{
2015-09-16 03:59:45 -03:00
float rssi_value_range = fabsf ( high_rssi_range - low_rssi_range ) ;
if ( is_zero ( rssi_value_range ) ) {
2015-08-28 02:59:17 -03:00
// User range isn't meaningful, return 0 for RSSI (and avoid divide by zero)
return 0.0f ;
}
// Note that user-supplied ranges may be inverted and we accommodate that here.
// (Some radio receivers put out inverted ranges for RSSI-type values).
bool range_is_inverted = ( high_rssi_range < low_rssi_range ) ;
// Constrain to the possible range - values outside are clipped to ends
current_rssi_value = constrain_float ( current_rssi_value ,
range_is_inverted ? high_rssi_range : low_rssi_range ,
range_is_inverted ? low_rssi_range : high_rssi_range ) ;
2015-09-16 03:58:38 -03:00
2015-08-28 02:59:17 -03:00
if ( range_is_inverted )
{
// Swap values so we can treat them as low->high uniformly in the code that follows
2016-04-22 21:37:06 -03:00
current_rssi_value = high_rssi_range + fabsf ( current_rssi_value - low_rssi_range ) ;
2015-08-28 02:59:17 -03:00
std : : swap ( low_rssi_range , high_rssi_range ) ;
}
2015-09-16 03:58:38 -03:00
2015-08-28 02:59:17 -03:00
// Scale the value down to a 0.0 - 1.0 range
float rssi_value_scaled = ( current_rssi_value - low_rssi_range ) / rssi_value_range ;
// Make absolutely sure the value is clipped to the 0.0 - 1.0 range. This should handle things if the
// value retrieved falls outside the user-supplied range.
return constrain_float ( rssi_value_scaled , 0.0f , 1.0f ) ;
}
2018-05-02 07:16:35 -03:00
2019-02-10 01:04:49 -04:00
AP_RSSI * AP_RSSI : : _singleton = nullptr ;
2018-05-02 07:16:35 -03:00
namespace AP {
AP_RSSI * rssi ( )
{
2019-02-10 01:04:49 -04:00
return AP_RSSI : : get_singleton ( ) ;
2018-05-02 07:16:35 -03:00
}
} ;