Plane: Moving to RSSI library for reading various kinds of RSSI, with the possibility of adding more.

* Retains ability to read from Analog Pin
* Adds ability to read RSSI from PWM channel value as is done in OpenLRSng, EazyUHF, and various other LRS.
* Handles any type of RSSI that provides RSSI values inverted - i.e. when the low value is the best signal and the high value is the worst signal.
* Has different key names from all existing RSSI parameters to provide for a clean break and easier distinguishing.
* Existing parameters are marked as obsolete
This commit is contained in:
Stewart Loving-Gibbard 2015-08-27 23:01:32 -07:00 committed by Andrew Tridgell
parent 7cb494d8e2
commit 722dd29370
6 changed files with 20 additions and 36 deletions

View File

@ -88,7 +88,7 @@ void Plane::setup()
notify.init(false);
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
rssi.init();
init_ardupilot();

View File

@ -912,21 +912,6 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
GSCALAR(override_channel, "OVERRIDE_CHAN", 0),
#endif
// @Param: RSSI_PIN
// @DisplayName: Receiver RSSI sensing pin
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
// @Values: -1:Disabled, 0:APM2 A0, 1:APM2 A1, 13:APM2 A13, 103:Pixhawk SBUS
// @User: Standard
GSCALAR(rssi_pin, "RSSI_PIN", -1),
// @Param: RSSI_RANGE
// @DisplayName: Receiver RSSI voltage range
// @Description: Receiver RSSI voltage range
// @Units: Volt
// @Values: 3.3:3.3V, 5.0:5V
// @User: Standard
GSCALAR(rssi_range, "RSSI_RANGE", 5.0),
// @Param: INVERTEDFLT_CH
// @DisplayName: Inverted flight channel
// @Description: A RC input channel number to enable inverted flight. If this is non-zero then the APM will monitor the corresponding RC input channel and will enable inverted flight when the channel goes above 1750.
@ -1204,6 +1189,10 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
// @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
GOBJECTN(EKF, NavEKF, "EKF_", NavEKF),
#endif
// @Group: RSSI_
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
AP_VAREND
};

View File

@ -101,7 +101,7 @@ public:
k_param_sonar_old, // unused
k_param_log_bitmask,
k_param_BoardConfig,
k_param_rssi_range,
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_flapin_channel,
k_param_flaperon_output,
k_param_gps,
@ -141,6 +141,9 @@ public:
k_param_gcs_pid_mask,
k_param_crash_detection_enable,
// 97: RSSI
k_param_rssi = 97,
// 100: Arming parameters
k_param_arming = 100,
@ -222,9 +225,9 @@ public:
// Battery monitoring parameters
//
k_param_battery = 166,
k_param_rssi_pin,
k_param_battery_volt_pin, // unused
k_param_battery_curr_pin, // unused - 169
k_param_rssi_pin, // unused, replaced by rssi_ library parameters - 167
k_param_battery_volt_pin, // unused - 168
k_param_battery_curr_pin, // unused - 169
//
// 170: Radio settings
@ -460,9 +463,7 @@ public:
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
AP_Int8 land_flap_percent;
AP_Int8 takeoff_flap_percent;
AP_Int8 rssi_pin;
AP_Float rssi_range; // allows to set max voltage for rssi pin such as 5.0, 3.3 etc.
AP_Int8 takeoff_flap_percent;
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 stick_mixing;
AP_Float takeoff_throttle_min_speed;

View File

@ -91,6 +91,7 @@
#include <AP_Rally/AP_Rally.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
// Configuration
#include "config.h"
@ -248,10 +249,6 @@ private:
// selected navigation controller
AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
// Analog Inputs
// a pin for reading the receiver RSSI voltage.
AP_HAL::AnalogSource *rssi_analog_source;
// Relay
AP_Relay relay;
@ -270,6 +267,9 @@ private:
// Rally Ponints
AP_Rally rally {ahrs};
// RSSI
AP_RSSI rssi;
// remember if USB is connected, so we can adjust baud rate
bool usb_connected;

View File

@ -47,6 +47,7 @@ LIBRARIES += AP_Frsky_Telem
LIBRARIES += AP_ServoRelayEvents
LIBRARIES += AP_Rally
LIBRARIES += AP_OpticalFlow
LIBRARIES += AP_RSSI
LIBRARIES += AP_HAL_AVR
LIBRARIES += AP_HAL_SITL
LIBRARIES += AP_HAL_PX4

View File

@ -1,6 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
#include <AP_RSSI/AP_RSSI.h>
void Plane::init_barometer(void)
{
@ -82,18 +83,10 @@ void Plane::read_battery(void)
}
}
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void Plane::read_receiver_rssi(void)
{
// avoid divide by zero
if (g.rssi_range <= 0) {
receiver_rssi = 0;
}else{
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->voltage_average() * 255 / g.rssi_range;
receiver_rssi = constrain_int16(ret, 0, 255);
}
receiver_rssi = rssi.read_receiver_rssi_uint8();
}