From 722dd2937081c9d0d0a624114fda89172a41c67d Mon Sep 17 00:00:00 2001 From: Stewart Loving-Gibbard Date: Thu, 27 Aug 2015 23:01:32 -0700 Subject: [PATCH] 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 --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Parameters.cpp | 19 ++++--------------- ArduPlane/Parameters.h | 15 ++++++++------- ArduPlane/Plane.h | 8 ++++---- ArduPlane/make.inc | 1 + ArduPlane/sensors.cpp | 11 ++--------- 6 files changed, 20 insertions(+), 36 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 95b9f4fcce..0cd341fb4c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -88,7 +88,7 @@ void Plane::setup() notify.init(false); - rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); + rssi.init(); init_ardupilot(); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 7b5a209b7d..f100d417f5 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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 }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9ce2b68c4f..6f1cf881bd 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 7fa46ac1a8..ee1a524c21 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -91,6 +91,7 @@ #include #include // Optical Flow library +#include // 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; diff --git a/ArduPlane/make.inc b/ArduPlane/make.inc index b5976628ad..af0b34db1a 100644 --- a/ArduPlane/make.inc +++ b/ArduPlane/make.inc @@ -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 diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 30b8d4bc43..faa294856d 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -1,6 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Plane.h" +#include 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(); }